media: rockchip: isp: add the iqtool module

Change-Id: Icf3a383e8dba6ea79ce112ddb32c194d70c3fb3a
Signed-off-by: Lian Xu <xu.lian@rock-chips.com>

drivers/media/platform/rockchip/isp/capture.c

@@ -902,7 +902,26 @@ static int rkisp_set_fmt(struct rkisp_stream *stream,
 	const struct stream_config *config = stream->config;
 	struct rkisp_device *dev = stream->ispdev;
 	u32 planes, imagsize = 0;
-	u32 i, xsubs = 1, ysubs = 1;
+	u32 i = 0, xsubs = 1, ysubs = 1;
+
+	if (stream->id == RKISP_STREAM_VIR) {
+		for (i = RKISP_STREAM_MP; i < RKISP_STREAM_VIR; i++) {
+			struct rkisp_stream *t = &dev->cap_dev.stream[i];
+
+			if (t->out_isp_fmt.fmt_type != FMT_YUV || !t->streaming)
+				continue;
+			if (t->out_fmt.plane_fmt[0].sizeimage > imagsize) {
+				imagsize = t->out_fmt.plane_fmt[0].sizeimage;
+				*pixm = t->out_fmt;
+				stream->conn_id = t->id;
+			}
+		}
+		if (!imagsize) {
+			v4l2_err(&dev->v4l2_dev, "no output stream for iqtool\n");
+			return -EINVAL;
+		}
+		imagsize = 0;
+	}
 
 	fmt = find_fmt(stream, pixm->pixelformat);
 	if (!fmt) {

drivers/media/platform/rockchip/isp/capture.h

@@ -45,6 +45,7 @@
 #define MP_VDEV_NAME DRIVER_NAME	"_mainpath"
 #define FBC_VDEV_NAME DRIVER_NAME	"_fbcpath"
 #define BP_VDEV_NAME DRIVER_NAME	"_fullpath"
+#define VIR_VDEV_NAME DRIVER_NAME	"_iqtool"
 
 #define DMATX0_VDEV_NAME DRIVER_NAME	"_rawwr0"
 #define DMATX1_VDEV_NAME DRIVER_NAME	"_rawwr1"
@@ -69,6 +70,7 @@ enum {
 	RKISP_STREAM_DMATX3,
 	RKISP_STREAM_FBC,
 	RKISP_STREAM_BP,
+	RKISP_STREAM_VIR,
 	RKISP_MAX_STREAM,
 };
 
@@ -249,6 +251,7 @@ struct rkisp_stream {
 	unsigned int burst;
 	atomic_t sequence;
 	struct frame_debug_info dbg;
+	u8 conn_id;
 	u32 memory;
 	union {
 		struct rkisp_stream_sp sp;
@@ -258,10 +261,18 @@ struct rkisp_stream {
 	} u;
 };
 
+struct rkisp_vir_cpy {
+	struct work_struct work;
+	struct completion cmpl;
+	struct list_head queue;
+	struct rkisp_stream *stream;
+};
+
 struct rkisp_capture_device {
 	struct rkisp_device *ispdev;
 	struct rkisp_stream stream[RKISP_MAX_STREAM];
 	struct rkisp_buffer *rdbk_buf[RDBK_MAX];
+	struct rkisp_vir_cpy vir_cpy;
 	atomic_t refcnt;
 	u32 wait_line;
 	bool is_done_early;

drivers/media/platform/rockchip/isp/capture_v30.c

@@ -711,7 +711,11 @@ static int mi_frame_end(struct rkisp_stream *stream)
 	unsigned long lock_flags = 0;
 	int i = 0;
 
+	if (stream->id == RKISP_STREAM_VIR)
+		return 0;
+
 	if (stream->curr_buf) {
+		struct rkisp_stream *vir = &dev->cap_dev.stream[RKISP_STREAM_VIR];
 		struct vb2_buffer *vb2_buf = &stream->curr_buf->vb.vb2_buf;
 		u64 ns = 0;
 
@@ -734,7 +738,23 @@ static int mi_frame_end(struct rkisp_stream *stream)
 		stream->dbg.id = stream->curr_buf->vb.sequence;
 		stream->dbg.delay = ns - dev->isp_sdev.frm_timestamp;
 
-		vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE);
+		if (vir->streaming && vir->conn_id == stream->id) {
+
+			spin_lock_irqsave(&vir->vbq_lock, lock_flags);
+			if (vir->streaming)
+				list_add_tail(&stream->curr_buf->queue,
+					&dev->cap_dev.vir_cpy.queue);
+			spin_unlock_irqrestore(&vir->vbq_lock, lock_flags);
+			if (!completion_done(&dev->cap_dev.vir_cpy.cmpl))
+				complete(&dev->cap_dev.vir_cpy.cmpl);
+
+			if (!vir->streaming)
+				vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE);
+
+		} else {
+			vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE);
+		}
+
 		stream->curr_buf = NULL;
 	}
 
@@ -964,6 +984,20 @@ static void rkisp_stop_streaming(struct vb2_queue *queue)
 	if (!stream->streaming)
 		goto end;
 
+	if (stream->id == RKISP_STREAM_VIR) {
+		stream->stopping = true;
+		wait_event_timeout(stream->done,
+				   stream->frame_end,
+				   msecs_to_jiffies(500));
+		stream->streaming = false;
+		stream->stopping = false;
+		destroy_buf_queue(stream, VB2_BUF_STATE_ERROR);
+
+		if (!completion_done(&dev->cap_dev.vir_cpy.cmpl))
+			complete(&dev->cap_dev.vir_cpy.cmpl);
+		goto end;
+	}
+
 	rkisp_stream_stop(stream);
 	/* call to the other devices */
 	media_pipeline_stop(&node->vdev.entity);
@@ -984,6 +1018,97 @@ end:
 	mutex_unlock(&dev->hw_dev->dev_lock);
 }
 
+static void vir_cpy_image(struct work_struct *work)
+{
+	struct rkisp_vir_cpy *cpy =
+	container_of(work, struct rkisp_vir_cpy, work);
+	struct rkisp_stream *vir = cpy->stream;
+	struct rkisp_buffer *src_buf = NULL;
+	unsigned long lock_flags = 0;
+	u32 i;
+
+	v4l2_dbg(1, rkisp_debug, &vir->ispdev->v4l2_dev,
+		 "%s enter\n", __func__);
+
+	vir->streaming = true;
+	spin_lock_irqsave(&vir->vbq_lock, lock_flags);
+	if (!list_empty(&cpy->queue)) {
+		src_buf = list_first_entry(&cpy->queue,
+				struct rkisp_buffer, queue);
+		list_del(&src_buf->queue);
+	}
+	spin_unlock_irqrestore(&vir->vbq_lock, lock_flags);
+
+	while (src_buf || vir->streaming) {
+		if (vir->stopping || !vir->streaming)
+			goto end;
+
+		if (!src_buf)
+			wait_for_completion(&cpy->cmpl);
+
+		vir->frame_end = false;
+		spin_lock_irqsave(&vir->vbq_lock, lock_flags);
+
+		if (!src_buf && !list_empty(&cpy->queue)) {
+			src_buf = list_first_entry(&cpy->queue,
+					struct rkisp_buffer, queue);
+			list_del(&src_buf->queue);
+		}
+
+		if (src_buf && !vir->curr_buf && !list_empty(&vir->buf_queue)) {
+			vir->curr_buf = list_first_entry(&vir->buf_queue,
+					struct rkisp_buffer, queue);
+			list_del(&vir->curr_buf->queue);
+		}
+		spin_unlock_irqrestore(&vir->vbq_lock, lock_flags);
+
+		if (!vir->curr_buf || !src_buf)
+			goto end;
+
+		for (i = 0; i < vir->out_isp_fmt.mplanes; i++) {
+			u32 payload_size = vir->out_fmt.plane_fmt[i].sizeimage;
+			void *src = vb2_plane_vaddr(&src_buf->vb.vb2_buf, i);
+			void *dst = vb2_plane_vaddr(&vir->curr_buf->vb.vb2_buf, i);
+
+			if (!src || !dst)
+				break;
+			vb2_set_plane_payload(&vir->curr_buf->vb.vb2_buf, i, payload_size);
+			memcpy(dst, src, payload_size);
+		}
+
+		vir->curr_buf->vb.sequence = src_buf->vb.sequence;
+		vir->curr_buf->vb.vb2_buf.timestamp = src_buf->vb.vb2_buf.timestamp;
+		vb2_buffer_done(&vir->curr_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+		vir->curr_buf = NULL;
+end:
+		if (src_buf)
+			vb2_buffer_done(&src_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+		src_buf = NULL;
+		spin_lock_irqsave(&vir->vbq_lock, lock_flags);
+
+		if (!list_empty(&cpy->queue)) {
+			src_buf = list_first_entry(&cpy->queue,
+					struct rkisp_buffer, queue);
+			list_del(&src_buf->queue);
+		} else if (vir->stopping) {
+			vir->streaming = false;
+		}
+
+		spin_unlock_irqrestore(&vir->vbq_lock, lock_flags);
+	}
+
+	vir->frame_end = true;
+
+	if (vir->stopping) {
+		vir->stopping = false;
+		vir->streaming = false;
+		wake_up(&vir->done);
+	}
+
+	v4l2_dbg(1, rkisp_debug, &vir->ispdev->v4l2_dev,
+		 "%s exit\n", __func__);
+}
+
 static int rkisp_stream_start(struct rkisp_stream *stream)
 {
 	struct v4l2_device *v4l2_dev = &stream->ispdev->v4l2_dev;
@@ -1036,6 +1161,28 @@ rkisp_start_streaming(struct vb2_queue *queue, unsigned int count)
 		return -EBUSY;
 	}
 
+	if (stream->id == RKISP_STREAM_VIR) {
+		struct rkisp_stream *t = &dev->cap_dev.stream[stream->conn_id];
+
+		if (t->streaming) {
+			INIT_WORK(&dev->cap_dev.vir_cpy.work, vir_cpy_image);
+			init_completion(&dev->cap_dev.vir_cpy.cmpl);
+			INIT_LIST_HEAD(&dev->cap_dev.vir_cpy.queue);
+			dev->cap_dev.vir_cpy.stream = stream;
+			schedule_work(&dev->cap_dev.vir_cpy.work);
+			ret = 0;
+		} else {
+			v4l2_err(&dev->v4l2_dev,
+				 "no stream enable for iqtool\n");
+			destroy_buf_queue(stream, VB2_BUF_STATE_QUEUED);
+			ret = -EINVAL;
+		}
+
+		mutex_unlock(&dev->hw_dev->dev_lock);
+
+		return ret;
+	}
+
 	memset(&stream->dbg, 0, sizeof(stream->dbg));
 	atomic_inc(&dev->cap_dev.refcnt);
 	if (!dev->isp_inp || !stream->linked) {
@@ -1166,6 +1313,11 @@ static int rkisp_stream_init(struct rkisp_device *dev, u32 id)
 		stream->ops = &rkisp_bp_streams_ops;
 		stream->config = &rkisp_bp_stream_config;
 		break;
+	case RKISP_STREAM_VIR:
+		strscpy(vdev->name, VIR_VDEV_NAME, sizeof(vdev->name));
+		stream->ops = NULL;
+		stream->config = &rkisp_mp_stream_config;
+		break;
 	default:
 		strscpy(vdev->name, MP_VDEV_NAME, sizeof(vdev->name));
 		stream->ops = &rkisp_mp_streams_ops;
@@ -1204,16 +1356,21 @@ int rkisp_register_stream_v30(struct rkisp_device *dev)
 	ret = rkisp_stream_init(dev, RKISP_STREAM_FBC);
 	if (ret < 0)
 		goto err_free_sp;
+	ret = rkisp_stream_init(dev, RKISP_STREAM_VIR);
+	if (ret < 0)
+		goto err_free_fbc;
 #ifdef RKISP_STREAM_BP_EN
 	ret = rkisp_stream_init(dev, RKISP_STREAM_BP);
 	if (ret < 0)
-		goto err_free_fbc;
+		goto err_free_vir;
 #endif
 	return 0;
 #ifdef RKISP_STREAM_BP_EN
+err_free_vir:
+	rkisp_unregister_stream_vdev(&cap_dev->stream[RKISP_STREAM_VIR]);
+#endif
 err_free_fbc:
 	rkisp_unregister_stream_vdev(&cap_dev->stream[RKISP_STREAM_FBC]);
-#endif
 err_free_sp:
 	rkisp_unregister_stream_vdev(&cap_dev->stream[RKISP_STREAM_SP]);
 err_free_mp:
@@ -1233,6 +1390,8 @@ void rkisp_unregister_stream_v30(struct rkisp_device *dev)
 	rkisp_unregister_stream_vdev(stream);
 	stream = &cap_dev->stream[RKISP_STREAM_FBC];
 	rkisp_unregister_stream_vdev(stream);
+	stream = &cap_dev->stream[RKISP_STREAM_VIR];
+	rkisp_unregister_stream_vdev(stream);
 #ifdef RKISP_STREAM_BP_EN
 	stream = &cap_dev->stream[RKISP_STREAM_BP];
 	rkisp_unregister_stream_vdev(stream);
@@ -1263,7 +1422,8 @@ void rkisp_mi_v30_isr(u32 mis_val, struct rkisp_device *dev)
 	for (i = 0; i < RKISP_MAX_STREAM; ++i) {
 		stream = &dev->cap_dev.stream[i];
 
-		if (!(mis_val & CIF_MI_FRAME(stream)))
+		if (!(mis_val & CIF_MI_FRAME(stream)) ||
+		    stream->id == RKISP_STREAM_VIR)
 			continue;
 
 		mi_frame_end_int_clear(stream);