diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c
index 3f4de09..80bc40b 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c
@@ -566,7 +566,7 @@ int sun6i_csi_update_config(struct sun6i_csi *csi,
 	return 0;
 }
 
-int sun6i_csi_update_buf_addr(struct sun6i_csi *csi, dma_addr_t addr)
+void sun6i_csi_update_buf_addr(struct sun6i_csi *csi, dma_addr_t addr)
 {
 	struct sun6i_csi_dev *sdev = sun6i_csi_to_dev(csi);
 	/* transform physical address to bus address */
@@ -580,11 +580,9 @@ int sun6i_csi_update_buf_addr(struct sun6i_csi *csi, dma_addr_t addr)
 	if (sdev->planar_offset[2] != -1)
 		regmap_write(sdev->regmap, CSI_CH_F2_BUFA_REG,
 			     (bus_addr + sdev->planar_offset[2]) >> 2);
-
-	return 0;
 }
 
-int sun6i_csi_set_stream(struct sun6i_csi *csi, bool enable)
+void sun6i_csi_set_stream(struct sun6i_csi *csi, bool enable)
 {
 	struct sun6i_csi_dev *sdev = sun6i_csi_to_dev(csi);
 	struct regmap *regmap = sdev->regmap;
@@ -592,7 +590,7 @@ int sun6i_csi_set_stream(struct sun6i_csi *csi, bool enable)
 	if (!enable) {
 		regmap_update_bits(regmap, CSI_CAP_REG, CSI_CAP_CH0_VCAP_ON, 0);
 		regmap_write(regmap, CSI_CH_INT_EN_REG, 0);
-		return 0;
+		return;
 	}
 
 	regmap_write(regmap, CSI_CH_INT_STA_REG, 0xFF);
@@ -606,8 +604,6 @@ int sun6i_csi_set_stream(struct sun6i_csi *csi, bool enable)
 
 	regmap_update_bits(regmap, CSI_CAP_REG, CSI_CAP_CH0_VCAP_ON,
 			   CSI_CAP_CH0_VCAP_ON);
-
-	return 0;
 }
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.h b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.h
index 12508ff..9bc758b 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.h
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.h
@@ -92,14 +92,14 @@ int sun6i_csi_update_config(struct sun6i_csi *csi,
  * @csi:	pointer to the csi
  * @addr:	frame buffer's physical address
  */
-int sun6i_csi_update_buf_addr(struct sun6i_csi *csi, dma_addr_t addr);
+void sun6i_csi_update_buf_addr(struct sun6i_csi *csi, dma_addr_t addr);
 
 /**
  * sun6i_csi_set_stream() - start/stop csi streaming
  * @csi:	pointer to the csi
  * @enable:	start/stop
  */
-int sun6i_csi_set_stream(struct sun6i_csi *csi, bool enable);
+void sun6i_csi_set_stream(struct sun6i_csi *csi, bool enable);
 
 static inline int v4l2_pixformat_get_bpp(unsigned int pixformat)
 {
diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.c b/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.c
index 0cebcbd..0819b71 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.c
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.c
@@ -29,6 +29,7 @@ struct sun6i_csi_buffer {
 	struct list_head		list;
 
 	dma_addr_t			dma_addr;
+	bool				queued_to_csi;
 };
 
 static struct sun6i_csi_format *
@@ -135,6 +136,7 @@ static int sun6i_video_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct sun6i_video *video = vb2_get_drv_priv(vq);
 	struct sun6i_csi_buffer *buf;
+	struct sun6i_csi_buffer *next_buf;
 	struct sun6i_csi_config config;
 	unsigned long flags;
 	int ret;
@@ -143,11 +145,7 @@ static int sun6i_video_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	ret = media_pipeline_start(&video->vdev.entity, &video->vdev.pipe);
 	if (ret < 0)
-		goto err_start_pipeline;
-
-	ret = sun6i_pipeline_set_stream(video, true);
-	if (ret < 0)
-		goto err_start_stream;
+		goto clear_dma_queue;
 
 	config.pixelformat = video->fmt.fmt.pix.pixelformat;
 	config.code = video->current_fmt->mbus_code;
@@ -157,31 +155,50 @@ static int sun6i_video_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	ret = sun6i_csi_update_config(video->csi, &config);
 	if (ret < 0)
-		goto err_update_config;
+		goto stop_media_pipeline;
 
 	spin_lock_irqsave(&video->dma_queue_lock, flags);
-	video->cur_frm = list_first_entry(&video->dma_queue,
-					  struct sun6i_csi_buffer, list);
-	list_del(&video->cur_frm->list);
-	spin_unlock_irqrestore(&video->dma_queue_lock, flags);
 
-	ret = sun6i_csi_update_buf_addr(video->csi, video->cur_frm->dma_addr);
-	if (ret < 0)
-		goto err_update_addr;
+	buf = list_first_entry(&video->dma_queue,
+			       struct sun6i_csi_buffer, list);
+	buf->queued_to_csi = true;
+	sun6i_csi_update_buf_addr(video->csi, buf->dma_addr);
+
+	sun6i_csi_set_stream(video->csi, true);
+
+	/* CSI will lookup the next dma buffer for next frame before the
+	 * the current frame done IRQ triggered. This is not documented
+	 * but reported by Ondřej Jirman.
+	 * The BSP code has workaround for this too. It skip to mark the
+	 * first buffer as frame done for VB2 and pass the second buffer
+	 * to CSI in the first frame done ISR call. Then in second frame
+	 * done ISR call, it mark the first buffer as frame done for VB2
+	 * and pass the third buffer to CSI. And so on. The bad thing is
+	 * that the first buffer will be written twice and the first frame
+	 * is dropped even the queued buffer is sufficient.
+	 * So, I make some improvement here. Pass the next buffer to CSI
+	 * just follow starting the CSI. In this case, the first frame
+	 * will be stored in first buffer, second frame in second buffer.
+	 * This mothed is used to avoid dropping the first frame, it
+	 * would also drop frame when lack of queued buffer.
+	 */
+	next_buf = list_next_entry(buf, list);
+	next_buf->queued_to_csi = true;
+	sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
 
-	ret = sun6i_csi_set_stream(video->csi, true);
+	spin_unlock_irqrestore(&video->dma_queue_lock, flags);
+
+	ret = sun6i_pipeline_set_stream(video, true);
 	if (ret < 0)
-		goto err_csi_stream;
+		goto stop_csi_stream;
 
 	return 0;
 
-err_csi_stream:
-err_update_addr:
-err_update_config:
-	sun6i_pipeline_set_stream(video, false);
-err_start_stream:
+stop_csi_stream:
+	sun6i_csi_set_stream(video->csi, false);
+stop_media_pipeline:
 	media_pipeline_stop(&video->vdev.entity);
-err_start_pipeline:
+clear_dma_queue:
 	spin_lock_irqsave(&video->dma_queue_lock, flags);
 	list_for_each_entry(buf, &video->dma_queue, list)
 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
@@ -205,11 +222,6 @@ static void sun6i_video_stop_streaming(struct vb2_queue *vq)
 
 	/* Release all active buffers */
 	spin_lock_irqsave(&video->dma_queue_lock, flags);
-	if (unlikely(video->cur_frm)) {
-		vb2_buffer_done(&video->cur_frm->vb.vb2_buf,
-				VB2_BUF_STATE_ERROR);
-		video->cur_frm = NULL;
-	}
 	list_for_each_entry(buf, &video->dma_queue, list)
 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 	INIT_LIST_HEAD(&video->dma_queue);
@@ -225,39 +237,58 @@ static void sun6i_video_buffer_queue(struct vb2_buffer *vb)
 	unsigned long flags;
 
 	spin_lock_irqsave(&video->dma_queue_lock, flags);
-	if (!video->cur_frm && list_empty(&video->dma_queue) &&
-		vb2_is_streaming(vb->vb2_queue)) {
-		video->cur_frm = buf;
-		sun6i_csi_update_buf_addr(video->csi, video->cur_frm->dma_addr);
-		sun6i_csi_set_stream(video->csi, 1);
-	} else
-		list_add_tail(&buf->list, &video->dma_queue);
+	buf->queued_to_csi = false;
+	list_add_tail(&buf->list, &video->dma_queue);
 	spin_unlock_irqrestore(&video->dma_queue_lock, flags);
 }
 
 void sun6i_video_frame_done(struct sun6i_video *video)
 {
+	struct sun6i_csi_buffer *buf;
+	struct sun6i_csi_buffer *next_buf;
+	struct vb2_v4l2_buffer *vbuf ;
+
 	spin_lock(&video->dma_queue_lock);
 
-	if (video->cur_frm) {
-		struct vb2_v4l2_buffer *vbuf = &video->cur_frm->vb;
-		struct vb2_buffer *vb = &vbuf->vb2_buf;
+	video->sequence++;
+
+	buf = list_first_entry(&video->dma_queue,
+			       struct sun6i_csi_buffer, list);
+	if (list_is_last(&buf->list, &video->dma_queue)) {
+		dev_dbg(video->csi->dev, "Frame droped!\n");
+		goto unlock;
+	}
 
-		vb->timestamp = ktime_get_ns();
-		vbuf->sequence = video->sequence++;
-		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
-		video->cur_frm = NULL;
+	next_buf = list_next_entry(buf, list);
+	/* If a new buffer (#next_buf) had not been queued to CSI, the old
+	 * buffer (#buf) is still holding by CSI for storing the next
+	 * frame. So, we queue a new buffer (#next_buf) to CSI then wait
+	 * for next ISR call.
+	 */
+	if (!next_buf->queued_to_csi) {
+		next_buf->queued_to_csi = true;
+		sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
+		dev_dbg(video->csi->dev, "Frame droped!\n");
+		goto unlock;
 	}
 
-	if (!list_empty(&video->dma_queue)
-	    && vb2_is_streaming(&video->vb2_vidq)) {
-		video->cur_frm = list_first_entry(&video->dma_queue,
-				struct sun6i_csi_buffer, list);
-		list_del(&video->cur_frm->list);
-		sun6i_csi_update_buf_addr(video->csi, video->cur_frm->dma_addr);
-	} else
-		sun6i_csi_set_stream(video->csi, 0);
+	list_del(&buf->list);
+	vbuf = &buf->vb;
+	vbuf->vb2_buf.timestamp = ktime_get_ns();
+	vbuf->sequence = video->sequence;
+	vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
 
+	if (list_is_last(&next_buf->list, &video->dma_queue))
+		goto unlock;
+
+	/* Prepare buffer for next frame but one.  */
+	next_buf = list_next_entry(next_buf, list);
+	if (!list_is_last(&next_buf->list, &video->dma_queue)) {
+		next_buf->queued_to_csi = true;
+		sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
+	}
+
+unlock:
 	spin_unlock(&video->dma_queue_lock);
 }
 
@@ -664,7 +695,6 @@ int sun6i_video_init(struct sun6i_video *video, struct sun6i_csi *csi,
 	INIT_LIST_HEAD(&video->dma_queue);
 	spin_lock_init(&video->dma_queue_lock);
 
-	video->cur_frm = NULL;
 	video->sequence = 0;
 	video->num_formats = 0;
 
@@ -677,7 +707,7 @@ int sun6i_video_init(struct sun6i_video *video, struct sun6i_csi *csi,
 	vidq->mem_ops			= &vb2_dma_contig_memops;
 	vidq->timestamp_flags		= V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	vidq->lock			= &video->lock;
-	vidq->min_buffers_needed	= 1;
+	vidq->min_buffers_needed	= 2;
 	vidq->dev			= csi->dev;
 
 	ret = vb2_queue_init(vidq);
diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.h b/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.h
index 14eac6e..b5a3d34 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.h
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.h
@@ -43,7 +43,6 @@ struct sun6i_video {
 	spinlock_t			dma_queue_lock;
 	struct list_head		dma_queue;
 
-	struct sun6i_csi_buffer		*cur_frm;
 	unsigned int			sequence;
 
 	struct sun6i_csi_format		*formats;