media: rockchip: isp: add iqtool video for isp32 lite

Change-Id: If5d31fc2b75e9aa0980044c47a8e429cc7e2a1b5
Signed-off-by: Cai YiWei <cyw@rock-chips.com>

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

@@ -676,18 +676,24 @@ static int rkisp_set_fmt(struct rkisp_stream *stream,
 			pixm->height = max_rsz.height;
 		}
 	} else 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];
+		struct rkisp_stream *t;
 
-			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 (stream->conn_id != -1) {
+			t = &dev->cap_dev.stream[stream->conn_id];
+			*pixm = t->out_fmt;
+		} else {
+			for (i = RKISP_STREAM_MP; i < RKISP_STREAM_VIR; i++) {
+				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) {
+		if (stream->conn_id == -1) {
 			v4l2_err(&dev->v4l2_dev, "no output stream for iqtool\n");
 			return -EINVAL;
 		}
@@ -1052,10 +1058,11 @@ static int rkisp_get_stream_info(struct rkisp_stream *stream,
 	u32 id = 0;
 
 	rkisp_dmarx_get_frame(stream->ispdev, &id, NULL, NULL, true);
-	info->cur_frame_id = id;
+	info->cur_frame_id = stream->dbg.id;
 	info->input_frame_loss = dev->isp_sdev.dbg.frameloss;
 	info->output_frame_loss = stream->dbg.frameloss;
 	info->stream_on = stream->streaming;
+	info->stream_id = stream->id;
 	return 0;
 }
 
@@ -1167,6 +1174,28 @@ int rkisp_free_tb_stream_buf(struct rkisp_stream *stream)
 	return sd->ops->core->ioctl(sd, RKISP_CMD_FREE_SHARED_BUF, NULL);
 }
 
+static int rkisp_set_iqtool_connect_id(struct rkisp_stream *stream, int stream_id)
+{
+	struct rkisp_device *dev = stream->ispdev;
+
+	if (stream->id != RKISP_STREAM_VIR) {
+		v4l2_err(&dev->v4l2_dev, "only support for iqtool video\n");
+		goto err;
+	}
+
+	if (stream_id != RKISP_STREAM_MP &&
+	    stream_id != RKISP_STREAM_SP &&
+	    stream_id != RKISP_STREAM_BP) {
+		v4l2_err(&dev->v4l2_dev, "invalid connect stream id\n");
+		goto err;
+	}
+
+	stream->conn_id = stream_id;
+	return 0;
+err:
+	return -EINVAL;
+}
+
 static long rkisp_ioctl_default(struct file *file, void *fh,
 				bool valid_prio, unsigned int cmd, void *arg)
 {
@@ -1237,6 +1266,9 @@ static long rkisp_ioctl_default(struct file *file, void *fh,
 	case RKISP_CMD_FREE_TB_STREAM_BUF:
 		ret = rkisp_free_tb_stream_buf(stream);
 		break;
+	case RKISP_CMD_SET_IQTOOL_CONN_ID:
+		ret = rkisp_set_iqtool_connect_id(stream, *(int *)arg);
+		break;
 	default:
 		ret = -EINVAL;
 	}

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

@@ -285,7 +285,7 @@ struct rkisp_stream {
 	unsigned int burst;
 	atomic_t sequence;
 	struct frame_debug_info dbg;
-	u8 conn_id;
+	int conn_id;
 	u32 memory;
 	union {
 		struct rkisp_stream_sp sp;

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

@@ -1248,6 +1248,7 @@ static void rkisp_stop_streaming(struct vb2_queue *queue)
 
 		if (!completion_done(&dev->cap_dev.vir_cpy.cmpl))
 			complete(&dev->cap_dev.vir_cpy.cmpl);
+		stream->conn_id = -1;
 		goto end;
 	}
 
@@ -1572,6 +1573,7 @@ static int rkisp_stream_init(struct rkisp_device *dev, u32 id)
 		strscpy(vdev->name, VIR_VDEV_NAME, sizeof(vdev->name));
 		stream->ops = NULL;
 		stream->config = &rkisp_mp_stream_config;
+		stream->conn_id = -1;
 		break;
 	default:
 		strscpy(vdev->name, MP_VDEV_NAME, sizeof(vdev->name));

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

@@ -1080,8 +1080,6 @@ static void update_mi(struct rkisp_stream *stream)
 		/* wrap buf ENC */
 		if (dev->isp_ver == ISP_V32)
 			val += stream->out_fmt.plane_fmt[0].bytesperline * dev->cap_dev.wrap_line;
-		else
-			stream->dbg.frameloss++;
 		reg = stream->config->mi.cb_base_ad_init;
 		rkisp_write(dev, reg, val, false);
 		if (is_cr_cfg) {
@@ -1373,10 +1371,14 @@ static int mi_frame_end(struct rkisp_stream *stream)
 	unsigned long lock_flags = 0;
 	u32 i;
 
+	if (stream->id == RKISP_STREAM_VIR)
+		return 0;
+
 	set_mirror_flip(stream);
 
 	if (stream->curr_buf) {
 		struct vb2_buffer *vb2_buf = &stream->curr_buf->vb.vb2_buf;
+		struct rkisp_stream *vir = &dev->cap_dev.stream[RKISP_STREAM_VIR];
 
 		if (dev->skip_frame) {
 			spin_lock_irqsave(&stream->vbq_lock, lock_flags);
@@ -1391,10 +1393,20 @@ static int mi_frame_end(struct rkisp_stream *stream)
 			vb2_set_plane_payload(vb2_buf, i, payload_size);
 		}
 
-		if (vb2_buf->memory)
-			rkisp_stream_buf_done(stream, stream->curr_buf);
-		else
+		if (vb2_buf->memory) {
+			if (vir->streaming && vir->conn_id == stream->id) {
+				spin_lock_irqsave(&vir->vbq_lock, lock_flags);
+				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);
+			} else {
+				rkisp_stream_buf_done(stream, stream->curr_buf);
+			}
+		} else {
 			rkisp_rockit_buf_done(stream, ROCKIT_DVBM_END);
+		}
 	}
 next:
 	spin_lock_irqsave(&stream->vbq_lock, lock_flags);
@@ -1590,7 +1602,7 @@ static void rkisp_buf_queue(struct vb2_buffer *vb)
 
 	spin_lock_irqsave(&stream->vbq_lock, lock_flags);
 	/* single sensor with pingpong buf, update next if need */
-	if (stream->ispdev->hw_dev->is_single &&
+	if (dev->hw_dev->is_single &&
 	    stream->id != RKISP_STREAM_VIR &&
 	    stream->id != RKISP_STREAM_LUMA &&
 	    stream->streaming && !stream->next_buf) {
@@ -1680,7 +1692,7 @@ static void rkisp_stop_streaming(struct vb2_queue *queue)
 	if (!stream->streaming)
 		goto end;
 
-	if (stream->id == RKISP_STREAM_LUMA) {
+	if (stream->id == RKISP_STREAM_LUMA || stream->id == RKISP_STREAM_VIR) {
 		stream->stopping = true;
 		if (!dev->hw_dev->is_shutdown)
 			wait_event_timeout(stream->done,
@@ -1689,7 +1701,12 @@ static void rkisp_stop_streaming(struct vb2_queue *queue)
 		stream->streaming = false;
 		stream->stopping = false;
 		destroy_buf_queue(stream, VB2_BUF_STATE_ERROR);
-		tasklet_disable(&dev->cap_dev.rd_tasklet);
+		if (stream->id == RKISP_STREAM_LUMA) {
+			tasklet_disable(&dev->cap_dev.rd_tasklet);
+		} else if (!completion_done(&dev->cap_dev.vir_cpy.cmpl)) {
+			complete(&dev->cap_dev.vir_cpy.cmpl);
+			stream->conn_id = -1;
+		}
 		goto end;
 	}
 
@@ -1721,6 +1738,97 @@ end:
 	}
 }
 
+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 rkisp_device *dev = stream->ispdev;
@@ -1779,6 +1887,25 @@ 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));
 
 	if (stream->id == RKISP_STREAM_LUMA) {
@@ -1960,6 +2087,12 @@ static int rkisp_stream_init(struct rkisp_device *dev, u32 id)
 			     (unsigned long)stream);
 		tasklet_disable(&cap_dev->rd_tasklet);
 		break;
+	case RKISP_STREAM_VIR:
+		strscpy(vdev->name, VIR_VDEV_NAME, sizeof(vdev->name));
+		stream->ops = NULL;
+		stream->config = &rkisp_mp_stream_config;
+		stream->conn_id = -1;
+		break;
 	default:
 		strscpy(vdev->name, MP_VDEV_NAME, sizeof(vdev->name));
 		stream->ops = &rkisp_mp_streams_ops;
@@ -2015,6 +2148,10 @@ int rkisp_register_stream_v32(struct rkisp_device *dev)
 			goto err_free_bpds;
 		rkisp_dvbm_get(dev);
 		rkisp_rockit_dev_init(dev);
+	} else {
+		ret = rkisp_stream_init(dev, RKISP_STREAM_VIR);
+		if (ret < 0)
+			goto err_free_sp;
 	}
 	return 0;
 err_free_bpds:
@@ -2050,6 +2187,9 @@ void rkisp_unregister_stream_v32(struct rkisp_device *dev)
 		stream = &cap_dev->stream[RKISP_STREAM_LUMA];
 		rkisp_unregister_stream_vdev(stream);
 		rkisp_rockit_dev_deinit();
+	} else {
+		stream = &cap_dev->stream[RKISP_STREAM_VIR];
+		rkisp_unregister_stream_vdev(stream);
 	}
 }
 
@@ -2067,7 +2207,8 @@ void rkisp_mi_v32_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);
@@ -2081,13 +2222,13 @@ void rkisp_mi_v32_isr(u32 mis_val, struct rkisp_device *dev)
 		if (stream->curr_buf) {
 			stream->curr_buf->vb.sequence = seq;
 			stream->curr_buf->vb.vb2_buf.timestamp = ns;
-		}
-		ns = ktime_get_ns();
-		stream->dbg.interval = ns - stream->dbg.timestamp;
-		stream->dbg.delay = ns - dev->isp_sdev.frm_timestamp;
-		stream->dbg.timestamp = ns;
-		stream->dbg.id = seq;
 
+			ns = ktime_get_ns();
+			stream->dbg.interval = ns - stream->dbg.timestamp;
+			stream->dbg.delay = ns - dev->isp_sdev.frm_timestamp;
+			stream->dbg.timestamp = ns;
+			stream->dbg.id = seq;
+		}
 		if (stream->is_tb_s_info) {
 			struct rkisp_tb_stream_info *tb_info = &dev->tb_stream_info;
 			u32 idx;

include/uapi/linux/rkisp2-config.h

@@ -96,6 +96,9 @@
 
 #define RKISP_CMD_FREE_TB_STREAM_BUF \
 	_IO('V', BASE_VIDIOC_PRIVATE + 112)
+
+#define RKISP_CMD_SET_IQTOOL_CONN_ID \
+	_IOW('V', BASE_VIDIOC_PRIVATE + 113, int)
 /*************************************************************/
 
 #define ISP2X_ID_DPCC			(0)
@@ -375,6 +378,7 @@ struct rkisp_stream_info {
 	unsigned int input_frame_loss;
 	unsigned int output_frame_loss;
 	unsigned char stream_on;
+	unsigned char stream_id;
 } __attribute__ ((packed));
 
 /* struct rkisp_mirror_flip