media: rockchip: isp1: use tasklet to get 3A states

Change-Id: I48b86cf19dd203bad32e7412c722056a6c917059
Signed-off-by: Hu Kejun <william.hu@rock-chips.com>

2 files changed, 60 insertions, 49 deletions

diff --git a/drivers/media/platform/rockchip/isp1/isp_stats.c b/drivers/media/platform/rockchip/isp1/isp_stats.c
index db4711b18f31..998b93262999 100644
--- a/drivers/media/platform/rockchip/isp1/isp_stats.c
+++ b/drivers/media/platform/rockchip/isp1/isp_stats.c
@@ -148,9 +148,9 @@ static void rkisp1_stats_vb2_buf_queue(struct vb2_buffer *vb)
 
 	stats_buf->vaddr[0] = vb2_plane_vaddr(vb, 0);
 
-	mutex_lock(&stats_dev->wq_lock);
+	spin_lock_bh(&stats_dev->rd_lock);
 	list_add_tail(&stats_buf->queue, &stats_dev->stat);
-	mutex_unlock(&stats_dev->wq_lock);
+	spin_unlock_bh(&stats_dev->rd_lock);
 }
 
 static void rkisp1_stats_vb2_stop_streaming(struct vb2_queue *vq)
@@ -165,9 +165,9 @@ static void rkisp1_stats_vb2_stop_streaming(struct vb2_queue *vq)
 	stats_vdev->streamon = false;
 	spin_unlock_irqrestore(&stats_vdev->irq_lock, flags);
 
-	drain_workqueue(stats_vdev->readout_wq);
+	tasklet_disable(&stats_vdev->rd_tasklet);
 
-	mutex_lock(&stats_vdev->wq_lock);
+	spin_lock_bh(&stats_vdev->rd_lock);
 	for (i = 0; i < RKISP1_ISP_STATS_REQ_BUFS_MAX; i++) {
 		if (list_empty(&stats_vdev->stat))
 			break;
@@ -176,7 +176,7 @@ static void rkisp1_stats_vb2_stop_streaming(struct vb2_queue *vq)
 		list_del(&buf->queue);
 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 	}
-	mutex_unlock(&stats_vdev->wq_lock);
+	spin_unlock_bh(&stats_vdev->rd_lock);
 }
 
 static int
@@ -186,6 +186,8 @@ rkisp1_stats_vb2_start_streaming(struct vb2_queue *queue,
 	struct rkisp1_isp_stats_vdev *stats_vdev = queue->drv_priv;
 
 	stats_vdev->streamon = true;
+	kfifo_reset(&stats_vdev->rd_kfifo);
+	tasklet_enable(&stats_vdev->rd_tasklet);
 
 	return 0;
 }
@@ -464,14 +466,14 @@ rkisp1_stats_send_measurement(struct rkisp1_isp_stats_vdev *stats_vdev,
 		cur_frame_id = meas_work->frame_id;
 	}
 
-	mutex_lock(&stats_vdev->wq_lock);
+	spin_lock(&stats_vdev->rd_lock);
 	/* get one empty buffer */
 	if (!list_empty(&stats_vdev->stat)) {
 		cur_buf = list_first_entry(&stats_vdev->stat,
 					   struct rkisp1_buffer, queue);
 		list_del(&cur_buf->queue);
 	}
-	mutex_unlock(&stats_vdev->wq_lock);
+	spin_unlock(&stats_vdev->rd_lock);
 
 	if (!cur_buf)
 		return;
@@ -511,23 +513,28 @@ rkisp1_stats_send_measurement(struct rkisp1_isp_stats_vdev *stats_vdev,
 	vb2_buffer_done(&cur_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 }
 
-static void rkisp1_stats_readout_work(struct work_struct *work)
+static void rkisp1_stats_readout_task(unsigned long data)
 {
-	struct rkisp1_isp_readout_work *readout_work = container_of(work,
-						struct rkisp1_isp_readout_work,
-						work);
-	struct rkisp1_isp_stats_vdev *stats_vdev = readout_work->stats_vdev;
-
-	if (readout_work->readout == RKISP1_ISP_READOUT_MEAS)
-		rkisp1_stats_send_measurement(stats_vdev, readout_work);
+	unsigned int out = 0;
+	struct rkisp1_isp_readout_work work;
+	struct rkisp1_isp_stats_vdev *vdev =
+		(struct rkisp1_isp_stats_vdev *)data;
+
+	while (!kfifo_is_empty(&vdev->rd_kfifo)) {
+		out = kfifo_out(&vdev->rd_kfifo,
+				&work, sizeof(work));
+		if (!out)
+			break;
 
-	kfree(readout_work);
+		if (work.readout == RKISP1_ISP_READOUT_MEAS)
+			rkisp1_stats_send_measurement(vdev, &work);
+	}
 }
 
 int rkisp1_stats_isr(struct rkisp1_isp_stats_vdev *stats_vdev, u32 isp_ris)
 {
 	unsigned int isp_mis_tmp = 0;
-	struct rkisp1_isp_readout_work *work;
+	struct rkisp1_isp_readout_work work;
 	unsigned int cur_frame_id =
 		atomic_read(&stats_vdev->dev->isp_sdev.frm_sync_seq) - 1;
 #ifdef LOG_ISR_EXE_TIME
@@ -552,22 +559,18 @@ int rkisp1_stats_isr(struct rkisp1_isp_stats_vdev *stats_vdev, u32 isp_ris)
 		goto unlock;
 	if (isp_ris & (CIF_ISP_AWB_DONE | CIF_ISP_AFM_FIN | CIF_ISP_EXP_END |
 		CIF_ISP_HIST_MEASURE_RDY)) {
-		work = kzalloc(sizeof(struct rkisp1_isp_readout_work),
-				GFP_ATOMIC);
-		if (work) {
-			INIT_WORK(&work->work,
-				  rkisp1_stats_readout_work);
-			work->readout = RKISP1_ISP_READOUT_MEAS;
-			work->stats_vdev = stats_vdev;
-			work->frame_id = cur_frame_id;
-			work->isp_ris = isp_ris;
-			if (!queue_work(stats_vdev->readout_wq,
-					&work->work))
-				kfree(work);
-		} else {
+		work.readout = RKISP1_ISP_READOUT_MEAS;
+		work.frame_id = cur_frame_id;
+		work.isp_ris = isp_ris;
+
+		if (!kfifo_is_full(&stats_vdev->rd_kfifo))
+			kfifo_in(&stats_vdev->rd_kfifo,
+				 &work, sizeof(work));
+		else
 			v4l2_err(stats_vdev->vnode.vdev.v4l2_dev,
-				 "Could not allocate work\n");
-		}
+				 "stats kfifo is full\n");
+
+		tasklet_schedule(&stats_vdev->rd_tasklet);
 	}
 
 #ifdef LOG_ISR_EXE_TIME
@@ -616,10 +619,10 @@ int rkisp1_register_stats_vdev(struct rkisp1_isp_stats_vdev *stats_vdev,
 	struct video_device *vdev = &node->vdev;
 
 	stats_vdev->dev = dev;
-	mutex_init(&stats_vdev->wq_lock);
 	mutex_init(&node->vlock);
 	INIT_LIST_HEAD(&stats_vdev->stat);
 	spin_lock_init(&stats_vdev->irq_lock);
+	spin_lock_init(&stats_vdev->rd_lock);
 
 	strlcpy(vdev->name, "rkisp1-statistics", sizeof(vdev->name));
 
@@ -648,18 +651,23 @@ int rkisp1_register_stats_vdev(struct rkisp1_isp_stats_vdev *stats_vdev,
 		goto err_cleanup_media_entity;
 	}
 
-	stats_vdev->readout_wq =
-	    alloc_workqueue("measurement_queue",
-			    WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
-
-	if (!stats_vdev->readout_wq) {
-		ret = -ENOMEM;
-			goto err_unreg_vdev;
+	ret = kfifo_alloc(&stats_vdev->rd_kfifo,
+			  RKISP1_READOUT_WORK_SIZE,
+			  GFP_KERNEL);
+	if (ret) {
+		dev_err(&vdev->dev,
+			"kfifo_alloc failed with error %d\n",
+			ret);
+		goto err_cleanup_media_entity;
 	}
 
+	tasklet_init(&stats_vdev->rd_tasklet,
+		     rkisp1_stats_readout_task,
+		     (unsigned long)stats_vdev);
+	tasklet_disable(&stats_vdev->rd_tasklet);
+
 	return 0;
-err_unreg_vdev:
-	video_unregister_device(vdev);
+
 err_cleanup_media_entity:
 	media_entity_cleanup(&vdev->entity);
 err_release_queue:
@@ -672,7 +680,8 @@ void rkisp1_unregister_stats_vdev(struct rkisp1_isp_stats_vdev *stats_vdev)
 	struct rkisp1_vdev_node *node = &stats_vdev->vnode;
 	struct video_device *vdev = &node->vdev;
 
-	destroy_workqueue(stats_vdev->readout_wq);
+	kfifo_free(&stats_vdev->rd_kfifo);
+	tasklet_kill(&stats_vdev->rd_tasklet);
 	video_unregister_device(vdev);
 	media_entity_cleanup(&vdev->entity);
 	vb2_queue_release(vdev->queue);
diff --git a/drivers/media/platform/rockchip/isp1/isp_stats.h b/drivers/media/platform/rockchip/isp1/isp_stats.h
index 1f674bc3a3fb..433502bee6a7 100644
--- a/drivers/media/platform/rockchip/isp1/isp_stats.h
+++ b/drivers/media/platform/rockchip/isp1/isp_stats.h
@@ -36,8 +36,13 @@
 #define _RKISP1_ISP_STATS_H
 
 #include <linux/rkisp1-config.h>
+#include <linux/interrupt.h>
+#include <linux/kfifo.h>
 #include "common.h"
 
+#define RKISP1_READOUT_WORK_SIZE	\
+	(8 * sizeof(struct rkisp1_isp_readout_work))
+
 struct rkisp1_isp_stats_vdev;
 
 enum rkisp1_isp_readout_cmd {
@@ -46,13 +51,9 @@ enum rkisp1_isp_readout_cmd {
 };
 
 struct rkisp1_isp_readout_work {
-	struct work_struct work;
-	struct rkisp1_isp_stats_vdev *stats_vdev;
-
 	unsigned int frame_id;
 	unsigned int isp_ris;
 	enum rkisp1_isp_readout_cmd readout;
-	struct vb2_buffer *vb;
 };
 
 struct rkisp1_stats_ops {
@@ -91,8 +92,9 @@ struct rkisp1_isp_stats_vdev {
 	struct v4l2_format vdev_fmt;
 	bool streamon;
 
-	struct workqueue_struct *readout_wq;
-	struct mutex wq_lock;
+	spinlock_t rd_lock;
+	struct kfifo rd_kfifo;
+	struct tasklet_struct rd_tasklet;
 
 	struct rkisp1_stats_ops *ops;
 	struct rkisp1_stats_config *config;