Reapply "media: tegra_camera: introduce 2 kthreads for capture"

This reverts commit b2f6b2abc791802631ff1c85488cdf3c1f0a97e2.

Signed-off-by: Mariusz Glebocki <mglebocki@antmicro.com>
Acked-by: Marcel Ziswiler <marcel.ziswiler@toradex.com>

4 files changed, 151 insertions, 101 deletions

diff --git a/drivers/media/platform/soc_camera/tegra_camera/common.c b/drivers/media/platform/soc_camera/tegra_camera/common.c
index 888d0aed96d3..06741f3ebca1 100644
--- a/drivers/media/platform/soc_camera/tegra_camera/common.c
+++ b/drivers/media/platform/soc_camera/tegra_camera/common.c
@@ -46,8 +46,6 @@ module_param(tpg_mode, int, 0644);
 #define TEGRA_CAM_DRV_NAME "vi"
 #define TEGRA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
 
-#define TEGRA_SYNCPT_RETRY_COUNT	10
-
 static const struct soc_mbus_pixelfmt tegra_camera_yuv_formats[] = {
 	{
 		.fourcc			= V4L2_PIX_FMT_UYVY,
@@ -203,73 +201,100 @@ static void tegra_camera_deactivate(struct tegra_camera_dev *cam)
 	cam->cal_done = 0;
 }
 
-static int tegra_camera_capture_frame(struct tegra_camera_dev *cam)
+static int tegra_camera_capture_frame(struct tegra_camera_dev *cam,
+				      struct tegra_camera_buffer *buf)
 {
-	struct vb2_buffer *vb = cam->active;
-	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
 	struct soc_camera_device *icd = buf->icd;
 	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
 	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
 	int port = pdata->port;
-	int retry = TEGRA_SYNCPT_RETRY_COUNT;
 	int err;
 
 	/* Setup capture registers */
-	cam->ops->capture_setup(cam);
+	cam->ops->capture_setup(cam, buf);
 
 	cam->ops->incr_syncpts(cam);
 
 	/* MIPI CSI pads calibration after starting capture */
-	if (cam->ops->mipi_calibration && !cam->cal_done) {
-		err = cam->ops->mipi_calibration(cam);
+	if (cam->ops->mipi_calibration && !cam->cal_done[port-1]) {
+		err = cam->ops->mipi_calibration(cam, buf);
 		if (!err)
 			cam->cal_done = 1;
 	}
 
-	while (retry) {
-		err = cam->ops->capture_start(cam, buf);
-		if (err) {
-			retry--;
+	/* Issue start capture */
+	cam->ops->capture_start(cam, buf);
+
+	/* Move buffer to capture done queue */
+	spin_lock(&cam->done_lock);
+	list_add_tail(&buf->queue, &cam->done);
+	spin_unlock(&cam->done_lock);
+
+	/* Wait up kthread for capture done */
+	wake_up_interruptible(&cam->capture_done_wait);
+
+	/* Wait for next frame start */
+	return cam->ops->capture_wait(cam, buf);
+}
+
+static int tegra_camera_kthread_capture_start(void *data)
+{
+	struct tegra_camera_dev *cam = data;
+	struct tegra_camera_buffer *buf;
 
-			cam->ops->incr_syncpts(cam);
-			if (cam->ops->save_syncpts)
-				cam->ops->save_syncpts(cam);
+	while (1) {
+		try_to_freeze();
 
+		wait_event_interruptible(cam->capture_start_wait,
+					 !list_empty(&cam->capture) ||
+					 kthread_should_stop());
+		if (kthread_should_stop())
+			break;
+
+		spin_lock(&cam->capture_lock);
+		if (list_empty(&cam->capture)) {
+			spin_unlock(&cam->capture_lock);
 			continue;
 		}
-		break;
-	}
 
-	/* Reset hardware for too many errors */
-	if (!retry) {
-		tegra_camera_deactivate(cam);
-		mdelay(5);
-		tegra_camera_activate(cam, icd);
-		if (cam->active)
-			cam->ops->capture_setup(cam);
+		buf = list_entry(cam->capture.next, struct tegra_camera_buffer,
+				 queue);
+		list_del_init(&buf->queue);
+		spin_unlock(&cam->capture_lock);
+
+		tegra_camera_capture_frame(cam, buf);
 	}
 
-	spin_lock(&cam->videobuf_queue_lock);
+	return 0;
+}
+
+static int tegra_camera_capture_done(struct tegra_camera_dev *cam,
+				     struct tegra_camera_buffer *buf)
+{
+	struct vb2_buffer *vb = &buf->vb;
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	int err;
+
+	/* Wait for buffer is output to memeory  */
+	err = cam->ops->capture_done(cam, port);
 
-	vb = cam->active;
+	/* Buffer is done */
 	do_gettimeofday(&vb->v4l2_buf.timestamp);
 	vb->v4l2_buf.field = cam->field;
 	if (port == TEGRA_CAMERA_PORT_CSI_A)
 		vb->v4l2_buf.sequence = cam->sequence_a++;
 	else if (port == TEGRA_CAMERA_PORT_CSI_B)
 		vb->v4l2_buf.sequence = cam->sequence_b++;
-
 	vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
-	list_del_init(&buf->queue);
-
 	cam->num_frames++;
 
-	spin_unlock(&cam->videobuf_queue_lock);
-
 	return err;
 }
 
-static int tegra_camera_kthread_capture(void *data)
+static int tegra_camera_kthread_capture_done(void *data)
 {
 	struct tegra_camera_dev *cam = data;
 	struct tegra_camera_buffer *buf;
@@ -277,25 +302,24 @@ static int tegra_camera_kthread_capture(void *data)
 	while (1) {
 		try_to_freeze();
 
-		wait_event_interruptible(cam->wait,
-					 !list_empty(&cam->capture) ||
+		wait_event_interruptible(cam->capture_done_wait,
+					 !list_empty(&cam->done) ||
 					 kthread_should_stop());
-		if (kthread_should_stop())
+		if (kthread_should_stop() && list_empty(&cam->done))
 			break;
 
-		spin_lock(&cam->videobuf_queue_lock);
-		if (list_empty(&cam->capture)) {
-			cam->active = NULL;
-			spin_unlock(&cam->videobuf_queue_lock);
+		spin_lock(&cam->done_lock);
+		if (list_empty(&cam->done)) {
+			spin_unlock(&cam->done_lock);
 			continue;
 		}
 
-		buf = list_entry(cam->capture.next, struct tegra_camera_buffer,
+		buf = list_entry(cam->done.next, struct tegra_camera_buffer,
 				 queue);
-		cam->active = &buf->vb;
-		spin_unlock(&cam->videobuf_queue_lock);
+		list_del_init(&buf->queue);
+		spin_unlock(&cam->done_lock);
 
-		tegra_camera_capture_frame(cam);
+		tegra_camera_capture_done(cam, buf);
 	}
 
 	return 0;
@@ -487,12 +511,12 @@ static void tegra_camera_videobuf_queue(struct vb2_buffer *vb)
 	struct tegra_camera_dev *cam = ici->priv;
 	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
 
-	spin_lock(&cam->videobuf_queue_lock);
+	spin_lock(&cam->capture_lock);
 	list_add_tail(&buf->queue, &cam->capture);
-	spin_unlock(&cam->videobuf_queue_lock);
+	spin_unlock(&cam->capture_lock);
 
 	/* Wait up kthread for capture */
-	wake_up_interruptible(&cam->wait);
+	wake_up_interruptible(&cam->capture_start_wait);
 }
 
 static void tegra_camera_videobuf_release(struct vb2_buffer *vb)
@@ -504,10 +528,7 @@ static void tegra_camera_videobuf_release(struct vb2_buffer *vb)
 	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
 	struct tegra_camera_dev *cam = ici->priv;
 
-	spin_lock(&cam->videobuf_queue_lock);
-
-	if (cam->active == vb)
-		cam->active = NULL;
+	spin_lock(&cam->done_lock);
 
 	/*
 	 * Doesn't hurt also if the list is empty, but it hurts, if queuing the
@@ -516,7 +537,7 @@ static void tegra_camera_videobuf_release(struct vb2_buffer *vb)
 	if (buf->queue.next)
 		list_del_init(&buf->queue);
 
-	spin_unlock(&cam->videobuf_queue_lock);
+	spin_unlock(&cam->done_lock);
 }
 
 static int tegra_camera_videobuf_init(struct vb2_buffer *vb)
@@ -535,9 +556,15 @@ static int tegra_camera_start_streaming(struct vb2_queue *q, unsigned int count)
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct tegra_camera_dev *cam = ici->priv;
 
-	/* Start kthread to capture data to buffer */
-	cam->kthread_capture = kthread_run(tegra_camera_kthread_capture, cam,
-					   dev_name(&cam->ndev->dev));
+	/* Start kthread to capture frame */
+	cam->kthread_capture_start = kthread_run(
+					tegra_camera_kthread_capture_start, cam,
+					"tegra-vi/capture-start");
+
+	/* Start kthread to wait data output to buffer */
+	cam->kthread_capture_done = kthread_run(
+					tegra_camera_kthread_capture_done, cam,
+					"tegra-vi/capture-done");
 	return 0;
 }
 
@@ -550,12 +577,13 @@ static int tegra_camera_stop_streaming(struct vb2_queue *q)
 	struct tegra_camera_dev *cam = ici->priv;
 	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
 	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
-	struct tegra_camera_buffer *buf, *nbuf;
 	int port = pdata->port;
 
 	/* Stop the kthread for capture */
-	kthread_stop(cam->kthread_capture);
-	cam->kthread_capture = NULL;
+	kthread_stop(cam->kthread_capture_start);
+	cam->kthread_capture_start = NULL;
+	kthread_stop(cam->kthread_capture_done);
+	cam->kthread_capture_done = NULL;
 
 	cam->ops->capture_stop(cam, port);
 
@@ -913,8 +941,11 @@ static int tegra_camera_probe(struct platform_device *pdev)
 	cam->tpg_mode = tpg_mode;
 
 	INIT_LIST_HEAD(&cam->capture);
-	spin_lock_init(&cam->videobuf_queue_lock);
-	init_waitqueue_head(&cam->wait);
+	INIT_LIST_HEAD(&cam->done);
+	spin_lock_init(&cam->capture_lock);
+	spin_lock_init(&cam->done_lock);
+	init_waitqueue_head(&cam->capture_start_wait);
+	init_waitqueue_head(&cam->capture_done_wait);
 
 	if (pdev->dev.of_node) {
 		int cplen;
diff --git a/drivers/media/platform/soc_camera/tegra_camera/common.h b/drivers/media/platform/soc_camera/tegra_camera/common.h
index c27c83cd4558..1d71c2ba39f5 100644
--- a/drivers/media/platform/soc_camera/tegra_camera/common.h
+++ b/drivers/media/platform/soc_camera/tegra_camera/common.h
@@ -62,9 +62,13 @@ struct tegra_camera_ops {
 	void (*clks_disable)(struct tegra_camera_dev *cam);
 
 	void (*capture_clean)(struct tegra_camera_dev *vi2_cam);
-	int (*capture_setup)(struct tegra_camera_dev *vi2_cam);
+	int (*capture_setup)(struct tegra_camera_dev *vi2_cam,
+			     struct tegra_camera_buffer *buf);
 	int (*capture_start)(struct tegra_camera_dev *vi2_cam,
 			     struct tegra_camera_buffer *buf);
+	int (*capture_wait)(struct tegra_camera_dev *vi2_cam,
+			     struct tegra_camera_buffer *buf);
+	int (*capture_done)(struct tegra_camera_dev *vi2_cam, int port);
 	int (*capture_stop)(struct tegra_camera_dev *vi2_cam, int port);
 
 	void (*init_syncpts)(struct tegra_camera_dev *vi2_cam);
@@ -76,7 +80,8 @@ struct tegra_camera_ops {
 	void (*deactivate)(struct tegra_camera_dev *vi2_cam);
 	int (*port_is_valid)(int port);
 
-	int (*mipi_calibration)(struct tegra_camera_dev *vi2_cam);
+	int (*mipi_calibration)(struct tegra_camera_dev *vi2_cam,
+			        struct tegra_camera_buffer *buf);
 };
 
 struct tegra_camera_dev {
@@ -93,16 +98,20 @@ struct tegra_camera_dev {
 	struct tegra_camera_ops		*ops;
 
 	void __iomem			*reg_base;
-	spinlock_t			videobuf_queue_lock;
 	struct list_head		capture;
+	spinlock_t			capture_lock;
+	struct list_head		done;
+	spinlock_t			done_lock;
 	struct vb2_buffer		*active;
 	struct vb2_alloc_ctx		*alloc_ctx;
 	enum v4l2_field			field;
 	int				sequence_a;
 	int				sequence_b;
 
-	struct task_struct		*kthread_capture;
-	wait_queue_head_t		wait;
+	struct task_struct		*kthread_capture_start;
+	struct task_struct		*kthread_capture_done;
+	wait_queue_head_t		capture_start_wait;
+	wait_queue_head_t		capture_done_wait;
 
 	/* syncpt ids */
 	u32				syncpt_id_csi_a;
diff --git a/drivers/media/platform/soc_camera/tegra_camera/vi.c b/drivers/media/platform/soc_camera/tegra_camera/vi.c
index 1fa0d098a88a..fe4ea6dcda8a 100644
--- a/drivers/media/platform/soc_camera/tegra_camera/vi.c
+++ b/drivers/media/platform/soc_camera/tegra_camera/vi.c
@@ -773,10 +773,9 @@ static int vi_capture_output_channel_setup(
 }
 
 
-static int vi_capture_setup(struct tegra_camera_dev *cam)
+static int vi_capture_setup(struct tegra_camera_dev *cam,
+			    struct tegra_camera_buffer *buf)
 {
-	struct vb2_buffer *vb = cam->active;
-	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
 	struct soc_camera_device *icd = buf->icd;
 	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
 	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
@@ -985,8 +984,7 @@ static int vi_capture_start(struct tegra_camera_dev *cam,
 
 static int vi_capture_stop(struct tegra_camera_dev *cam, int port)
 {
-	struct tegra_camera_buffer *buf = to_tegra_vb(cam->active);
-	int err;
+	int err = 0;
 
 	if (vi_port_is_csi(port))
 		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
@@ -995,8 +993,6 @@ static int vi_capture_stop(struct tegra_camera_dev *cam, int port)
 			TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
 			NULL,
 			NULL);
-	else
-		err = 0;
 
 	if (err) {
 		u32 buffer_addr;
@@ -1005,18 +1001,8 @@ static int vi_capture_stop(struct tegra_camera_dev *cam, int port)
 
 		dev_warn(&cam->ndev->dev, "Timeout on VI syncpt\n");
 
-		if (buf->output_channel == 0)
-			buffer_addr = TC_VI_REG_RD(cam,
+		buffer_addr = TC_VI_REG_RD(cam,
 					   TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
-		else if (buf->output_channel == 1)
-			buffer_addr = TC_VI_REG_RD(cam,
-					   TEGRA_VI_VB0_BASE_ADDRESS_SECOND);
-		else {
-			dev_err(&cam->ndev->dev, "Wrong output channel %d\n",
-				buf->output_channel);
-			return -EINVAL;
-		}
-
 		dev_warn(&cam->ndev->dev, "buffer_addr = 0x%08x\n",
 			buffer_addr);
 
diff --git a/drivers/media/platform/soc_camera/tegra_camera/vi2.c b/drivers/media/platform/soc_camera/tegra_camera/vi2.c
index 56659fe6bb3a..99bef12e1b7f 100644
--- a/drivers/media/platform/soc_camera/tegra_camera/vi2.c
+++ b/drivers/media/platform/soc_camera/tegra_camera/vi2.c
@@ -787,10 +787,9 @@ static int vi2_capture_setup_csi_1(struct tegra_camera_dev *cam,
 	return 0;
 }
 
-static int vi2_capture_setup(struct tegra_camera_dev *cam)
+static int vi2_capture_setup(struct tegra_camera_dev *cam,
+			     struct tegra_camera_buffer *buf)
 {
-	struct vb2_buffer *vb = cam->active;
-	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
 	struct soc_camera_device *icd = buf->icd;
 	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
 	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
@@ -960,7 +959,7 @@ static void vi2_capture_error_status(struct tegra_camera_dev *cam)
 }
 
 static int vi2_capture_start(struct tegra_camera_dev *cam,
-				      struct tegra_camera_buffer *buf)
+			     struct tegra_camera_buffer *buf)
 {
 	struct soc_camera_device *icd = buf->icd;
 	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
@@ -973,7 +972,6 @@ static int vi2_capture_start(struct tegra_camera_dev *cam,
 	if (err < 0)
 		return err;
 
-	/* Only wait on CSI frame end syncpt if we're using CSI. */
 	if (port == TEGRA_CAMERA_PORT_CSI_A) {
 		if (!nvhost_syncpt_read_ext_check(cam->ndev,
 					cam->syncpt_id_csi_a, &val))
@@ -984,13 +982,6 @@ static int vi2_capture_start(struct tegra_camera_dev *cam,
 		TC_VI_REG_WT(cam, TEGRA_VI_CFG_VI_INCR_SYNCPT,
 			     VI_CSI_PPA_FRAME_START | cam->syncpt_id_csi_a);
 		TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SINGLE_SHOT, 0x1);
-
-		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
-				cam->syncpt_id_csi_a,
-				cam->syncpt_csi_a,
-				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
-				NULL,
-				NULL);
 	} else if (port == TEGRA_CAMERA_PORT_CSI_B ||
 			port == TEGRA_CAMERA_PORT_CSI_C) {
 		if (!nvhost_syncpt_read_ext_check(cam->ndev,
@@ -1002,7 +993,30 @@ static int vi2_capture_start(struct tegra_camera_dev *cam,
 		TC_VI_REG_WT(cam, TEGRA_VI_CFG_VI_INCR_SYNCPT,
 			     VI_CSI_PPB_FRAME_START | cam->syncpt_id_csi_b);
 		TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_SINGLE_SHOT, 0x1);
+	}
+
+	return err;
+}
+
+static int vi2_capture_wait(struct tegra_camera_dev *cam,
+			    struct tegra_camera_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	int err = 0;
 
+	/* Only wait on CSI frame end syncpt if we're using CSI. */
+	if (port == TEGRA_CAMERA_PORT_CSI_A) {
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+				cam->syncpt_id_csi_a,
+				cam->syncpt_csi_a,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL,
+				NULL);
+	} else if (port == TEGRA_CAMERA_PORT_CSI_B ||
+			port == TEGRA_CAMERA_PORT_CSI_C) {
 		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
 				cam->syncpt_id_csi_b,
 				cam->syncpt_csi_b,
@@ -1032,7 +1046,7 @@ static int vi2_capture_start(struct tegra_camera_dev *cam,
 	return err;
 }
 
-static int vi2_capture_stop(struct tegra_camera_dev *cam, int port)
+static int vi2_capture_done(struct tegra_camera_dev *cam, int port)
 {
 	u32 val;
 	int err = 0;
@@ -1062,7 +1076,6 @@ static int vi2_capture_stop(struct tegra_camera_dev *cam, int port)
 				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
 				NULL,
 				NULL);
-		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0xf002);
 	} else if (port == TEGRA_CAMERA_PORT_CSI_B ||
 			port == TEGRA_CAMERA_PORT_CSI_C) {
 		if (!nvhost_syncpt_read_ext_check(cam->ndev,
@@ -1089,12 +1102,22 @@ static int vi2_capture_stop(struct tegra_camera_dev *cam, int port)
 				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
 				NULL,
 				NULL);
-		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0xf002);
 	}
 
 	return err;
 }
 
+static int vi2_capture_stop(struct tegra_camera_dev *cam, int port)
+{
+	u32 reg = (port == TEGRA_CAMERA_PORT_CSI_A) ?
+		  TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND :
+		  TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND;
+
+	TC_VI_REG_WT(cam, reg, 0xf002);
+
+	return 0;
+}
+
 /* Reset VI2/CSI2 when activating, no sepecial ops for deactiving  */
 static void vi2_sw_reset(struct tegra_camera_dev *cam)
 {
@@ -1107,13 +1130,12 @@ static void vi2_sw_reset(struct tegra_camera_dev *cam)
 
 }
 
-static int vi2_mipi_calibration(struct tegra_camera_dev *cam)
+static int vi2_mipi_calibration(struct tegra_camera_dev *cam,
+				struct tegra_camera_buffer *buf)
 {
 	void __iomem *mipi_cal;
 	struct regmap *regs;
 	struct platform_device *pdev = cam->ndev;
-	struct vb2_buffer *vb = cam->active;
-	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
 	struct soc_camera_device *icd = buf->icd;
 	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
 	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
@@ -1256,6 +1278,8 @@ struct tegra_camera_ops vi2_ops = {
 	.capture_clean = vi2_capture_clean,
 	.capture_setup = vi2_capture_setup,
 	.capture_start = vi2_capture_start,
+	.capture_wait = vi2_capture_wait,
+	.capture_done = vi2_capture_done,
 	.capture_stop = vi2_capture_stop,
 
 	.activate = vi2_sw_reset,