media: rockchip: isp: thunderboot for isp32

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

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

@@ -11,6 +11,7 @@
 #include <media/videobuf2-dma-contig.h>
 #include "dev.h"
 #include "regs.h"
+#include "rkisp_tb_helper.h"
 
 #define STREAM_MAX_MP_RSZ_OUTPUT_WIDTH		4416
 #define STREAM_MAX_MP_RSZ_OUTPUT_HEIGHT		3312
@@ -1143,6 +1144,7 @@ int rkisp_fh_open(struct file *filp)
 	struct rkisp_stream *stream = video_drvdata(filp);
 	int ret;
 
+	stream->is_using_resmem = false;
 	ret = v4l2_fh_open(filp);
 	if (!ret) {
 		ret = v4l2_pipeline_pm_get(&stream->vnode.vdev.entity);
@@ -1449,16 +1451,14 @@ static int rkisp_set_wrap_line(struct rkisp_stream *stream, int *line)
 {
 	struct rkisp_device *dev = stream->ispdev;
 
-	if (dev->isp_ver != ISP_V32)
-		return -EINVAL;
-
-	if (dev->hw_dev->dev_link_num > 1 || stream->id != RKISP_STREAM_MP) {
+	if (dev->isp_ver != ISP_V32 ||
+	    dev->hw_dev->dev_link_num > 1 ||
+	    !stream->ops->set_wrap) {
 		v4l2_err(&dev->v4l2_dev,
 			 "wrap only support for single sensor and mainpath\n");
 		return -EINVAL;
 	}
-	dev->cap_dev.wrap_line = *line;
-	return 0;
+	return stream->ops->set_wrap(stream, *line);
 }
 
 static int rkisp_set_fps(struct rkisp_stream *stream, int *fps)
@@ -1796,6 +1796,32 @@ static const struct v4l2_ioctl_ops rkisp_v4l2_ioctl_ops = {
 	.vidioc_default = rkisp_ioctl_default,
 };
 
+static void rkisp_stream_fast(struct work_struct *work)
+{
+	struct rkisp_capture_device *cap_dev =
+		container_of(work, struct rkisp_capture_device, fast_work);
+	struct rkisp_stream *stream = &cap_dev->stream[0];
+	struct vb2_queue *q = &stream->vnode.buf_queue;
+	struct rkisp_device *ispdev = cap_dev->ispdev;
+
+	v4l2_pipeline_pm_get(&stream->vnode.vdev.entity);
+	rkisp_chk_tb_over(ispdev);
+	if (ispdev->tb_head.complete != RKISP_TB_OK) {
+		v4l2_pipeline_pm_put(&stream->vnode.vdev.entity);
+		return;
+	}
+	stream->is_pre_on = true;
+	stream->is_using_resmem = true;
+	ispdev->resmem_addr_curr = ispdev->resmem_addr;
+	if (ispdev->resmem_size < stream->out_fmt.plane_fmt[0].sizeimage) {
+		stream->is_using_resmem = false;
+		v4l2_warn(&ispdev->v4l2_dev,
+			  "resmem size:%zu no enough for image:%d\n",
+			  ispdev->resmem_size, stream->out_fmt.plane_fmt[0].sizeimage);
+	}
+	q->ops->start_streaming(q, 1);
+}
+
 void rkisp_unregister_stream_vdev(struct rkisp_stream *stream)
 {
 	media_entity_cleanup(&stream->vnode.vdev.entity);
@@ -1902,6 +1928,8 @@ int rkisp_register_stream_vdevs(struct rkisp_device *dev)
 		st_cfg->max_rsz_height = CIF_ISP_INPUT_H_MAX_V32;
 		ret = rkisp_register_stream_v32(dev);
 	}
+
+	INIT_WORK(&cap_dev->fast_work, rkisp_stream_fast);
 	return ret;
 }
 

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

@@ -221,6 +221,7 @@ struct streams_ops {
 	void (*update_mi)(struct rkisp_stream *stream);
 	int (*frame_end)(struct rkisp_stream *stream);
 	int (*frame_start)(struct rkisp_stream *stream, u32 mis);
+	int (*set_wrap)(struct rkisp_stream *stream, int line);
 };
 
 struct rockit_isp_ops {
@@ -276,6 +277,8 @@ struct rkisp_stream {
 	bool is_flip;
 	bool is_pause;
 	bool is_crop_upd;
+	bool is_pre_on;
+	bool is_using_resmem;
 	wait_queue_head_t done;
 	unsigned int burst;
 	atomic_t sequence;
@@ -308,6 +311,8 @@ struct rkisp_capture_device {
 	u32 wrap_line;
 	bool is_done_early;
 	bool is_mirror;
+
+	struct work_struct fast_work;
 };
 
 extern struct stream_config rkisp_mp_stream_config;

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

@@ -26,6 +26,7 @@
 
 static int mi_frame_end(struct rkisp_stream *stream);
 static int mi_frame_start(struct rkisp_stream *stream, u32 mis);
+static int rkisp_create_dummy_buf(struct rkisp_stream *stream);
 
 static const struct capture_fmt bp_fmts[] = {
 	{
@@ -781,6 +782,7 @@ static void update_mi(struct rkisp_stream *stream)
 			stream->next_buf = NULL;
 		}
 	} else if (dummy_buf->mem_priv) {
+		/* wrap buf ENC */
 		val = dummy_buf->dma_addr;
 		reg = stream->config->mi.y_base_ad_init;
 		rkisp_write(dev, reg, val, false);
@@ -791,6 +793,22 @@ static void update_mi(struct rkisp_stream *stream)
 			reg = stream->config->mi.cr_base_ad_init;
 			rkisp_write(dev, reg, val, false);
 		}
+	} else if (stream->is_using_resmem) {
+		/* resmem for fast stream NV12 output */
+		dma_addr_t max_addr = dev->resmem_addr + dev->resmem_size;
+		u32 bytesperline = stream->out_fmt.plane_fmt[0].bytesperline;
+		u32 buf_size = bytesperline * ALIGN(stream->out_fmt.height, 16) * 3 / 2;
+
+		reg = stream->config->mi.y_base_ad_init;
+		val = dev->resmem_addr_curr;
+		rkisp_write(dev, reg, val, false);
+
+		reg = stream->config->mi.cb_base_ad_init;
+		val += bytesperline * stream->out_fmt.height;
+		rkisp_write(dev, reg, val, false);
+
+		if (dev->resmem_addr_curr + buf_size * 2 <= max_addr)
+			dev->resmem_addr_curr += buf_size;
 	} else if (!stream->is_pause) {
 		stream->is_pause = true;
 		stream->ops->disable_mi(stream);
@@ -933,6 +951,29 @@ static int luma_frame_end(struct rkisp_stream *stream)
 	return 0;
 }
 
+static int mp_set_wrap(struct rkisp_stream *stream, int line)
+{
+	struct rkisp_device *dev = stream->ispdev;
+	int ret = 0;
+
+	dev->cap_dev.wrap_line = line;
+	if (stream->is_pre_on &&
+	    stream->streaming &&
+	    !stream->dummy_buf.mem_priv) {
+		ret = rkisp_create_dummy_buf(stream);
+		if (ret)
+			return ret;
+		stream->ops->config_mi(stream);
+		if (stream->is_pause) {
+			stream->ops->enable_mi(stream);
+			if (!ISP3X_ISP_OUT_LINE(rkisp_read(dev, ISP3X_ISP_DEBUG2, true)))
+				stream_self_update(stream);
+			stream->is_pause = false;
+		}
+	}
+	return ret;
+}
+
 static struct streams_ops rkisp_mp_streams_ops = {
 	.config_mi = mp_config_mi,
 	.enable_mi = mp_enable_mi,
@@ -942,6 +983,7 @@ static struct streams_ops rkisp_mp_streams_ops = {
 	.update_mi = update_mi,
 	.frame_end = mi_frame_end,
 	.frame_start = mi_frame_start,
+	.set_wrap = mp_set_wrap,
 };
 
 static struct streams_ops rkisp_sp_streams_ops = {
@@ -1258,7 +1300,7 @@ static int rkisp_create_dummy_buf(struct rkisp_stream *stream)
 	buf->size = dev->isp_sdev.in_crop.width * dev->cap_dev.wrap_line * 2;
 	if (stream->out_isp_fmt.output_format == ISP32_MI_OUTPUT_YUV420)
 		buf->size = buf->size - buf->size / 4;
-
+	buf->size = stream->out_fmt.plane_fmt[0].sizeimage;
 	buf->is_need_dbuf = true;
 	ret = rkisp_alloc_buffer(stream->ispdev, buf);
 	if (ret == 0) {
@@ -1354,6 +1396,11 @@ static void rkisp_stop_streaming(struct vb2_queue *queue)
 
 end:
 	mutex_unlock(&dev->hw_dev->dev_lock);
+
+	if (stream->is_pre_on) {
+		stream->is_pre_on = false;
+		v4l2_pipeline_pm_put(&stream->vnode.vdev.entity);
+	}
 }
 
 static int rkisp_stream_start(struct rkisp_stream *stream)
@@ -1406,7 +1453,10 @@ rkisp_start_streaming(struct vb2_queue *queue, unsigned int count)
 
 	if (WARN_ON(stream->streaming)) {
 		mutex_unlock(&dev->hw_dev->dev_lock);
-		return -EBUSY;
+		if (stream->is_pre_on)
+			return 0;
+		else
+			return -EBUSY;
 	}
 
 	memset(&stream->dbg, 0, sizeof(stream->dbg));

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

@@ -413,10 +413,15 @@ static int _set_pipeline_default_fmt(struct rkisp_device *dev)
 	memset(&fmt, 0, sizeof(fmt));
 	isp = &dev->isp_sdev.sd;
 
-	if (dev->active_sensor)
+	if (dev->active_sensor) {
 		fmt = dev->active_sensor->fmt[0];
-	else
+		if (fmt.format.code == dev->isp_sdev.in_frm.code &&
+		    fmt.format.width == dev->isp_sdev.in_frm.width &&
+		    fmt.format.height == dev->isp_sdev.in_frm.height)
+			return 0;
+	} else {
 		fmt.format = dev->isp_sdev.in_frm;
+	}
 	code = fmt.format.code;
 	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 	fmt.pad = RKISP_ISP_PAD_SINK;
@@ -549,6 +554,8 @@ static int subdev_notifier_complete(struct v4l2_async_notifier *notifier)
 
 unlock:
 	mutex_unlock(&dev->media_dev.graph_mutex);
+	if (!ret && dev->is_thunderboot)
+		schedule_work(&dev->cap_dev.fast_work);
 	return ret;
 }
 
@@ -781,9 +788,8 @@ static int rkisp_get_reserved_mem(struct rkisp_device *isp_dev)
 					      sizeof(struct rkisp_thunderboot_resmem_head),
 					      DMA_BIDIRECTIONAL);
 	ret = dma_mapping_error(dev, isp_dev->resmem_addr);
-
-	dev_info(dev, "Allocated reserved memory, paddr: 0x%x\n",
-		(u32)isp_dev->resmem_pa);
+	isp_dev->is_thunderboot = true;
+	dev_info(dev, "Allocated reserved memory, paddr: 0x%x\n", (u32)isp_dev->resmem_pa);
 	return ret;
 }
 
@@ -830,9 +836,11 @@ static int rkisp_plat_probe(struct platform_device *pdev)
 	strscpy(isp_dev->media_dev.driver_name, isp_dev->name,
 		sizeof(isp_dev->media_dev.driver_name));
 
-	ret = rkisp_get_reserved_mem(isp_dev);
-	if (ret)
-		return ret;
+	if (isp_dev->hw_dev->is_thunderboot) {
+		ret = rkisp_get_reserved_mem(isp_dev);
+		if (ret)
+			return ret;
+	}
 
 	mutex_init(&isp_dev->apilock);
 	mutex_init(&isp_dev->iqlock);

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

@@ -212,7 +212,10 @@ struct rkisp_device {
 	wait_queue_head_t sync_onoff;
 	dma_addr_t resmem_addr;
 	phys_addr_t resmem_pa;
+	dma_addr_t resmem_addr_curr;
 	size_t resmem_size;
+	struct rkisp_thunderboot_resmem_head tb_head;
+	bool is_thunderboot;
 	int dev_id;
 	unsigned int skip_frame;
 	unsigned int irq_ends;

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

@@ -147,6 +147,7 @@ static void rkisp_params_vb2_buf_queue(struct vb2_buffer *vb)
 	if (params_vdev->first_params) {
 		first_param = vb2_plane_vaddr(vb, 0);
 		params_vdev->ops->save_first_param(params_vdev, first_param);
+		params_vdev->is_first_cfg = true;
 		vbuf->sequence = cur_frame_id;
 		vb2_buffer_done(&params_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 		params_vdev->first_params = false;
@@ -212,7 +213,6 @@ rkisp_params_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
 	struct rkisp_isp_params_vdev *params_vdev = queue->drv_priv;
 	unsigned long flags;
 
-	params_vdev->is_first_cfg = true;
 	params_vdev->hdrtmo_en = false;
 	params_vdev->afaemode_en = false;
 	params_vdev->cur_buf = NULL;

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

@@ -4684,6 +4684,13 @@ rkisp_params_isr_v32(struct rkisp_isp_params_vdev *params_vdev,
 	struct rkisp_device *dev = params_vdev->dev;
 	u32 cur_frame_id;
 
+	if (params_vdev->is_first_cfg) {
+		rkisp_params_first_cfg(params_vdev, &dev->isp_sdev.in_fmt,
+				       dev->isp_sdev.quantization);
+		rkisp_set_bits(dev, ISP3X_ISP_CTRL0, 0, CIF_ISP_CTRL_ISP_CFG_UPD, true);
+		return;
+	}
+
 	rkisp_dmarx_get_frame(dev, &cur_frame_id, NULL, NULL, true);
 	if (isp_mis & CIF_ISP_V_START) {
 		if (params_vdev->rdbk_times)

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

@@ -674,14 +674,13 @@ void rkisp_stats_first_ddr_config_v32(struct rkisp_isp_stats_vdev *stats_vdev)
 	struct rkisp_device *dev = stats_vdev->dev;
 	u32 size = stats_vdev->vdev_fmt.fmt.meta.buffersize;
 
-	if (!stats_vdev->streamon)
+	if (dev->isp_sdev.in_fmt.fmt_type == FMT_YUV)
 		return;
 
 	stats_vdev->stats_buf[0].is_need_vaddr = true;
 	stats_vdev->stats_buf[0].size = sizeof(struct rkisp32_isp_stat_buffer);
 	if (rkisp_alloc_buffer(dev, &stats_vdev->stats_buf[0]))
-		v4l2_warn(&dev->v4l2_dev,
-			  "stats alloc buf fail\n");
+		v4l2_warn(&dev->v4l2_dev, "stats alloc buf fail\n");
 	rkisp_stats_update_buf(stats_vdev);
 	rkisp_write(dev, ISP3X_MI_DBR_WR_SIZE, size, false);
 	rkisp_set_bits(dev, ISP3X_SWS_CFG, 0, ISP3X_3A_DDR_WRITE_EN, false);

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

@@ -380,7 +380,8 @@ int rkisp_update_sensor_info(struct rkisp_device *dev)
 	if (ret && ret != -ENOIOCTLCMD)
 		return ret;
 
-	if (sensor->mbus.type == V4L2_MBUS_CSI2_DPHY) {
+	if (sensor->mbus.type == V4L2_MBUS_CSI2_DPHY &&
+	    dev->isp_ver < ISP_V30) {
 		u8 vc = 0;
 
 		memset(dev->csi_dev.mipi_di, 0,
@@ -3050,6 +3051,7 @@ static long rkisp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 {
 	struct rkisp_device *isp_dev = sd_to_isp_dev(sd);
 	struct rkisp_thunderboot_resmem *resmem;
+	struct rkisp32_thunderboot_resmem_head *tb_head_v32;
 	struct rkisp_thunderboot_resmem_head *head;
 	struct rkisp_thunderboot_shmem *shmem;
 	struct isp2x_buf_idxfd *idxfd;
@@ -3066,6 +3068,18 @@ static long rkisp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 	case RKISP_CMD_GET_ISP_INFO:
 		rkisp_get_info(isp_dev, arg);
 		break;
+	case RKISP_CMD_GET_TB_HEAD_V32:
+		if (isp_dev->tb_head.complete != RKISP_TB_OK) {
+			ret = -EINVAL;
+			break;
+		}
+		tb_head_v32 = arg;
+		memcpy(tb_head_v32, &isp_dev->tb_head,
+		       sizeof(struct rkisp_thunderboot_resmem_head));
+		memcpy(&tb_head_v32->cfg, isp_dev->params_vdev.isp32_params,
+		       sizeof(struct isp32_isp_params_cfg));
+		isp_dev->tb_head.complete = 0;
+		break;
 	case RKISP_CMD_GET_SHARED_BUF:
 		if (!IS_ENABLED(CONFIG_VIDEO_ROCKCHIP_THUNDER_BOOT_ISP)) {
 			ret = -ENOIOCTLCMD;
@@ -3405,7 +3419,10 @@ void rkisp_chk_tb_over(struct rkisp_device *isp_dev)
 	enum rkisp_tb_state tb_state;
 	void *resmem_va;
 
-	if (!isp_dev->resmem_pa || !isp_dev->resmem_size) {
+	if (!isp_dev->hw_dev->is_thunderboot)
+		return;
+
+	if (!isp_dev->is_thunderboot) {
 		v4l2_info(&isp_dev->v4l2_dev,
 			  "no reserved memory for thunderboot\n");
 		if (isp_dev->hw_dev->is_thunderboot) {
@@ -3419,15 +3436,50 @@ void rkisp_chk_tb_over(struct rkisp_device *isp_dev)
 
 	resmem_va = phys_to_virt(isp_dev->resmem_pa);
 	head = (struct rkisp_thunderboot_resmem_head *)resmem_va;
-	if (isp_dev->hw_dev->is_thunderboot) {
-		shm_head_poll_timeout(isp_dev, !!head->enable, 2000, 200 * USEC_PER_MSEC);
-		shm_head_poll_timeout(isp_dev, !!head->complete, 5000, 500 * USEC_PER_MSEC);
-		if (head->complete != RKISP_TB_OK)
-			v4l2_info(&isp_dev->v4l2_dev,
-				  "wait thunderboot over timeout\n");
+	if (isp_dev->is_thunderboot) {
+		dma_sync_single_for_cpu(isp_dev->dev, isp_dev->resmem_addr,
+					sizeof(struct rkisp_thunderboot_resmem_head),
+					DMA_FROM_DEVICE);
+		if (head->enable && !head->complete) {
+			/* notify rtt to stop */
+			head->enable = 0;
+			dma_sync_single_for_device(isp_dev->dev, isp_dev->resmem_addr,
+						   sizeof(struct rkisp_thunderboot_resmem_head),
+						   DMA_FROM_DEVICE);
+		}
+		shm_head_poll_timeout(isp_dev, !!head->complete, 5000, 100 * USEC_PER_MSEC);
+		if (head->complete != RKISP_TB_OK) {
+			v4l2_err(&isp_dev->v4l2_dev, "wait thunderboot over timeout\n");
+		} else {
+			u32 size = 0;
 
+			switch (isp_dev->hw_dev->isp_ver) {
+			case ISP_V32:
+				size = sizeof(struct rkisp32_thunderboot_resmem_head);
+				break;
+			default:
+				break;
+			}
+			if (size && size < isp_dev->resmem_size) {
+				dma_sync_single_for_cpu(isp_dev->dev, isp_dev->resmem_addr,
+							size, DMA_FROM_DEVICE);
+				isp_dev->params_vdev.is_first_cfg = true;
+				if (isp_dev->hw_dev->isp_ver == ISP_V32) {
+					struct rkisp32_thunderboot_resmem_head *tmp = resmem_va;
+
+					memcpy(isp_dev->params_vdev.isp32_params, &tmp->cfg,
+					       sizeof(struct isp32_isp_params_cfg));
+				}
+			} else if (size > isp_dev->resmem_size) {
+				v4l2_err(&isp_dev->v4l2_dev,
+					 "resmem size:%zu no enough for head:%d\n",
+					 isp_dev->resmem_size, size);
+				head->complete = RKISP_TB_NG;
+			}
+		}
+		memcpy(&isp_dev->tb_head, head, sizeof(*head));
 		v4l2_info(&isp_dev->v4l2_dev,
-			  "thunderboot info: %d, %d, %d, %d, %d, %d, 0x%x\n",
+			  "thunderboot info: %d, %d, %d, %d, %d, %d %d\n",
 			  head->enable,
 			  head->complete,
 			  head->frm_total,
@@ -3446,6 +3498,7 @@ void rkisp_chk_tb_over(struct rkisp_device *isp_dev)
 		rkisp_tb_unprotect_clk();
 		rkisp_register_irq(isp_dev->hw_dev);
 		isp_dev->hw_dev->is_thunderboot = false;
+		isp_dev->is_thunderboot = false;
 	}
 }
 #endif

include/uapi/linux/rkisp2-config.h

@@ -50,6 +50,8 @@
 #define RKISP_CMD_MESHBUF_FREE \
 	_IOW('V', BASE_VIDIOC_PRIVATE + 11, long long)
 
+/* BASE_VIDIOC_PRIVATE + 12 for RKISP_CMD_GET_TB_HEAD_V32 */
+
 /****************ISP VIDEO IOCTL******************************/
 
 #define RKISP_CMD_GET_CSI_MEMORY_MODE \
@@ -1916,19 +1918,6 @@ struct rkisp_isp2x_luma_buffer {
 	struct rkisp_mipi_luma luma[ISP2X_MIPI_RAW_MAX];
 } __attribute__ ((packed));
 
-/**
- * struct rkisp_thunderboot_video_buf
- */
-struct rkisp_thunderboot_video_buf {
-	u32 index;
-	u32 frame_id;
-	u32 timestamp;
-	u32 time_reg;
-	u32 gain_reg;
-	u32 bufaddr;
-	u32 bufsize;
-} __attribute__ ((packed));
-
 /**
  * struct rkisp_thunderboot_resmem_head
  */
@@ -1941,9 +1930,10 @@ struct rkisp_thunderboot_resmem_head {
 	u16 height;
 	u32 bus_fmt;
 
-	struct rkisp_thunderboot_video_buf l_buf[ISP2X_THUNDERBOOT_VIDEO_BUF_NUM];
-	struct rkisp_thunderboot_video_buf m_buf[ISP2X_THUNDERBOOT_VIDEO_BUF_NUM];
-	struct rkisp_thunderboot_video_buf s_buf[ISP2X_THUNDERBOOT_VIDEO_BUF_NUM];
+	u32 exp_time[3];
+	u32 exp_gain[3];
+	u32 exp_time_reg[3];
+	u32 exp_gain_reg[3];
 } __attribute__ ((packed));
 
 /**

include/uapi/linux/rkisp32-config.h

@@ -11,6 +11,9 @@
 #include <linux/v4l2-controls.h>
 #include <linux/rkisp3-config.h>
 
+#define RKISP_CMD_GET_TB_HEAD_V32 \
+	_IOR('V', BASE_VIDIOC_PRIVATE + 12, struct rkisp32_thunderboot_resmem_head)
+
 #define ISP32_MODULE_DPCC		ISP3X_MODULE_DPCC
 #define ISP32_MODULE_BLS		ISP3X_MODULE_BLS
 #define ISP32_MODULE_SDG		ISP3X_MODULE_SDG
@@ -1397,4 +1400,8 @@ struct rkisp32_isp_stat_buffer {
 	u32 frame_id;
 } __attribute__ ((packed));
 
+struct rkisp32_thunderboot_resmem_head {
+	struct rkisp_thunderboot_resmem_head head;
+	struct isp32_isp_params_cfg cfg;
+};
 #endif /* _UAPI_RKISP32_CONFIG_H */