media: rockchip: vicap support wake up buffer with mode of rdbk by isp driver

in this mode,buffer wake up earlier, isp can process images earlier

Signed-off-by: Zefa Chen <zefa.chen@rock-chips.com>
Change-Id: I75da9ac768f17f6ebc745f57a32e6cd753f9be3e

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

@@ -2312,6 +2312,99 @@ static int rkcif_update_new_buffer_wake_up_mode(struct rkcif_stream *stream)
 	return ret;
 }
 
+static int rkcif_get_new_buffer_wake_up_mode_rdbk(struct rkcif_stream *stream)
+{
+	struct rkisp_rx_buf *dbufs = NULL;
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&stream->vbq_lock, flags);
+	if (!list_empty(&stream->rx_buf_head)) {
+		if (stream->line_int_cnt % 2) {
+			dbufs = list_first_entry(&stream->rx_buf_head,
+						 struct rkisp_rx_buf, list);
+			if (dbufs) {
+				list_del(&dbufs->list);
+				stream->curr_buf_toisp = to_cif_rx_buf(dbufs);
+			}
+		} else {
+			dbufs = list_first_entry(&stream->rx_buf_head,
+						 struct rkisp_rx_buf, list);
+			if (dbufs) {
+				list_del(&dbufs->list);
+				stream->next_buf_toisp = to_cif_rx_buf(dbufs);
+			}
+		}
+		stream->is_buf_active = true;
+		if (stream->lack_buf_cnt)
+			stream->lack_buf_cnt--;
+	} else {
+		if (stream->lack_buf_cnt < 2)
+			stream->lack_buf_cnt++;
+		if (stream->curr_buf_toisp && stream->next_buf_toisp &&
+		    stream->curr_buf_toisp != stream->next_buf_toisp) {
+			if (stream->line_int_cnt % 2)
+				stream->curr_buf_toisp = stream->next_buf_toisp;
+			else
+				stream->next_buf_toisp = stream->curr_buf_toisp;
+			stream->is_buf_active = true;
+			ret = 0;
+			if (stream->cifdev->rdbk_debug)
+				v4l2_info(&stream->cifdev->v4l2_dev,
+					  "hold buf %x\n",
+					  (u32)stream->next_buf_toisp->dummy.dma_addr);
+		} else {
+			stream->is_buf_active = false;
+			ret = -EINVAL;
+		}
+	}
+	spin_unlock_irqrestore(&stream->vbq_lock, flags);
+
+	return ret;
+}
+
+static int rkcif_update_new_buffer_wake_up_mode_rdbk(struct rkcif_stream *stream)
+{
+	struct rkcif_device *dev = stream->cifdev;
+	struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
+	struct rkcif_rx_buffer *buffer = NULL;
+	u32 frm_addr_y;
+	int channel_id = stream->id;
+	int ret = 0;
+	unsigned long flags;
+
+	if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY ||
+	    mbus_cfg->type == V4L2_MBUS_CSI2_CPHY ||
+	    mbus_cfg->type == V4L2_MBUS_CCP2) {
+		frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
+			     get_reg_index_of_frm0_y_addr(channel_id) :
+			     get_reg_index_of_frm1_y_addr(channel_id);
+	} else {
+		frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
+			     get_dvp_reg_index_of_frm0_y_addr(channel_id) :
+			     get_dvp_reg_index_of_frm1_y_addr(channel_id);
+	}
+	spin_lock_irqsave(&stream->vbq_lock, flags);
+	if (stream->is_buf_active) {
+		if (stream->frame_phase == CIF_CSI_FRAME0_READY)
+			buffer = stream->curr_buf_toisp;
+		else if (stream->frame_phase == CIF_CSI_FRAME1_READY)
+			buffer = stream->next_buf_toisp;
+	}
+	spin_unlock_irqrestore(&stream->vbq_lock, flags);
+	if (buffer) {
+		rkcif_write_register(dev, frm_addr_y,
+				     buffer->dummy.dma_addr);
+		if (dev->rdbk_debug > 1 &&
+		    stream->frame_idx < 15)
+			v4l2_info(&dev->v4l2_dev,
+				  "rdbk update, seq %d, reg %x, buf %x\n",
+				  stream->frame_idx - 1,
+				  frm_addr_y, (u32)buffer->dummy.dma_addr);
+	}
+	return ret;
+}
+
 static void rkcif_assign_dummy_buffer(struct rkcif_stream *stream)
 {
 	struct rkcif_device *dev = stream->cifdev;
@@ -3888,6 +3981,10 @@ void rkcif_do_stop_stream(struct rkcif_stream *stream,
 			rkcif_do_cru_reset(dev);
 			dev->can_be_reset = false;
 			dev->reset_work_cancel = true;
+			dev->early_line = 0;
+			dev->sensor_linetime = 0;
+			dev->wait_line = 0;
+			stream->is_line_wake_up = false;
 		}
 	}
 	if (!atomic_read(&dev->pipe.stream_cnt) && dev->dummy_buf.vaddr)
@@ -7276,6 +7373,46 @@ static void rkcif_line_wake_up(struct rkcif_stream *stream, int mipi_id)
 	rkcif_buf_done_prepare(stream, active_buf, mipi_id, mode);
 }
 
+static int rkcif_stop_dma_capture(struct rkcif_stream *stream);
+static void rkcif_line_wake_up_rdbk(struct rkcif_stream *stream, int mipi_id)
+{
+	u32 mode;
+	struct rkcif_rx_buffer *active_buf = NULL;
+	struct sditf_priv *priv = NULL;
+	int ret = 0;
+
+	mode = stream->line_int_cnt % 2;
+	if (mode) {
+		if (stream->curr_buf_toisp)
+			active_buf = stream->curr_buf_toisp;
+	} else {
+		if (stream->next_buf_toisp)
+			active_buf = stream->next_buf_toisp;
+	}
+
+	if (!active_buf) {
+		v4l2_err(&stream->cifdev->v4l2_dev,
+			 "err buffer state in %s\n",
+			 __func__);
+		return;
+	}
+
+	if (stream->stopping) {
+		stream->is_can_stop = true;
+		return;
+	}
+	ret = rkcif_get_new_buffer_wake_up_mode_rdbk(stream);
+	if (!ret) {
+		priv = stream->cifdev->sditf[0];
+		if (priv && priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO) {
+			active_buf->dbufs.sequence = stream->frame_idx - 1;
+			active_buf->dbufs.timestamp = stream->readout.fs_timestamp;
+			rkcif_s_rx_buffer(stream->cifdev, &active_buf->dbufs);
+			stream->buf_num_toisp--;
+		}
+	}
+}
+
 static void rkcif_deal_readout_time(struct rkcif_stream *stream)
 {
 	struct rkcif_device *cif_dev = stream->cifdev;
@@ -7378,9 +7515,12 @@ static void rkcif_update_stream_toisp(struct rkcif_device *cif_dev,
 	if (cif_dev->inf_id == RKCIF_MIPI_LVDS)
 		rkcif_deal_readout_time(stream);
 
-	rkcif_assign_new_buffer_pingpong_toisp(stream,
+	if (!stream->is_line_wake_up)
-					 RKCIF_YUV_ADDR_STATE_UPDATE,
+		rkcif_assign_new_buffer_pingpong_toisp(stream,
-					 mipi_id);
+						       RKCIF_YUV_ADDR_STATE_UPDATE,
+						       mipi_id);
+	else
+		rkcif_update_new_buffer_wake_up_mode_rdbk(stream);
 }
 
 static u32 rkcif_get_sof(struct rkcif_device *cif_dev)
@@ -7965,7 +8105,12 @@ int rkcif_reset_notifier(struct notifier_block *nb,
 static void rkcif_modify_line_int(struct rkcif_stream *stream, bool en)
 {
 	struct rkcif_device *cif_dev = stream->cifdev;
+	u32 line_intr_en = 0;
 
+	if (cif_dev->chip_id >= CHIP_RK3588_CIF)
+		line_intr_en = CSI_LINE_INTEN_RK3588(stream->id);
+	else
+		line_intr_en = CSI_LINE_INTEN(stream->id);
 	if (en) {
 		if (cif_dev->wait_line_bak != cif_dev->wait_line) {
 			cif_dev->wait_line_bak = cif_dev->wait_line;
@@ -7975,10 +8120,10 @@ static void rkcif_modify_line_int(struct rkcif_stream *stream, bool en)
 					     cif_dev->wait_line << 16 | cif_dev->wait_line);
 		}
 		rkcif_write_register_or(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
-					CSI_LINE_INTEN(stream->id));
+					line_intr_en);
 	} else {
 		rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
-					 ~(CSI_LINE_INTEN(stream->id)));
+					 ~line_intr_en);
 	}
 }
 
@@ -7990,6 +8135,18 @@ static void rkcif_detect_wake_up_mode_change(struct rkcif_stream *stream)
 	if (!priv || priv->mode.rdbk_mode == RKISP_VICAP_ONLINE)
 		return;
 
+	if ((cif_dev->hdr.hdr_mode == NO_HDR || cif_dev->hdr.hdr_mode == HDR_COMPR) &&
+	    stream->id == RKCIF_STREAM_MIPI_ID0) {
+		if (cif_dev->wait_line != cif_dev->wait_line_cache)
+			cif_dev->wait_line = cif_dev->wait_line_cache;
+	} else if (cif_dev->hdr.hdr_mode == HDR_X2 && stream->id == RKCIF_STREAM_MIPI_ID1) {
+		if (cif_dev->wait_line != cif_dev->wait_line_cache)
+			cif_dev->wait_line = cif_dev->wait_line_cache;
+	} else if (cif_dev->hdr.hdr_mode == HDR_X3 && stream->id == RKCIF_STREAM_MIPI_ID2) {
+		if (cif_dev->wait_line != cif_dev->wait_line_cache)
+			cif_dev->wait_line = cif_dev->wait_line_cache;
+	}
+
 	if (cif_dev->wait_line && (!stream->is_line_wake_up)) {
 		stream->is_line_wake_up = true;
 		if (stream->frame_phase == CIF_CSI_FRAME0_READY)
@@ -8003,18 +8160,6 @@ static void rkcif_detect_wake_up_mode_change(struct rkcif_stream *stream)
 		rkcif_modify_line_int(stream, true);
 		stream->is_line_inten = true;
 	}
-
-	if ((cif_dev->hdr.hdr_mode == NO_HDR || cif_dev->hdr.hdr_mode == HDR_COMPR) &&
-	    stream->id == RKCIF_STREAM_MIPI_ID0) {
-		if (cif_dev->wait_line != cif_dev->wait_line_cache)
-			cif_dev->wait_line = cif_dev->wait_line_cache;
-	} else if (cif_dev->hdr.hdr_mode == HDR_X2 && stream->id == RKCIF_STREAM_MIPI_ID1) {
-		if (cif_dev->wait_line != cif_dev->wait_line_cache)
-			cif_dev->wait_line = cif_dev->wait_line_cache;
-	} else if (cif_dev->hdr.hdr_mode == HDR_X3 && stream->id == RKCIF_STREAM_MIPI_ID2) {
-		if (cif_dev->wait_line != cif_dev->wait_line_cache)
-			cif_dev->wait_line = cif_dev->wait_line_cache;
-	}
 }
 
 u32 rkcif_mbus_pixelcode_to_v4l2(u32 pixelcode)
@@ -8710,11 +8855,14 @@ void rkcif_irq_pingpong_v1(struct rkcif_device *cif_dev)
 						stream->is_finish_stop_dma = true;
 				}
 			}
-			if (intstat & CSI_LINE_INTSTAT(i)) {
+			if (intstat & CSI_LINE_INTSTAT_V1(i)) {
 				stream = &cif_dev->stream[i];
 				if (stream->is_line_inten) {
 					stream->line_int_cnt++;
-					rkcif_line_wake_up(stream, stream->id);
+					if (cif_dev->sditf[0] && cif_dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO)
+						rkcif_line_wake_up_rdbk(stream, stream->id);
+					else
+						rkcif_line_wake_up(stream, stream->id);
 					rkcif_modify_line_int(stream, false);
 					stream->is_line_inten = false;
 				}

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

@@ -1848,6 +1848,7 @@ int rkcif_plat_init(struct rkcif_device *cif_dev, struct device_node *node, int
 	cif_dev->sditf_cnt = 0;
 	cif_dev->is_notifier_isp = false;
 	cif_dev->sensor_linetime = 0;
+	cif_dev->early_line = 0;
 	cif_dev->rdbk_debug = 0;
 	if (cif_dev->chip_id == CHIP_RV1126_CIF_LITE)
 		cif_dev->isr_hdl = rkcif_irq_lite_handler;

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

@@ -812,6 +812,8 @@ struct rkcif_device {
 	int				rdbk_debug;
 	int				sync_type;
 	int				sditf_cnt;
+	u32				early_line;
+	int				isp_runtime_max;
 	int				sensor_linetime;
 };
 

drivers/media/platform/rockchip/cif/subdev-itf.c

@@ -588,10 +588,12 @@ static int sditf_s_rx_buffer(struct v4l2_subdev *sd,
 {
 	struct sditf_priv *priv = to_sditf_priv(sd);
 	struct rkcif_device *cif_dev = priv->cif_dev;
+	struct rkcif_sensor_info *sensor = &cif_dev->terminal_sensor;
 	struct rkcif_stream *stream = NULL;
 	struct rkisp_rx_buf *dbufs;
 	struct rkcif_rx_buffer *rx_buf = NULL;
 	unsigned long flags;
+	u32 diff_time = 1000000;
 
 	if (!buf) {
 		v4l2_err(&cif_dev->v4l2_dev, "buf is NULL\n");
@@ -637,6 +639,72 @@ static int sditf_s_rx_buffer(struct v4l2_subdev *sd,
 	}
 
 	spin_unlock_irqrestore(&stream->vbq_lock, flags);
+
+	if (dbufs->runtime_us && cif_dev->early_line == 0) {
+		u32 numerator, denominator;
+		u32 def_fps = 0;
+		int line_time = 0;
+		int vblank_def = 0;
+		int vblank_curr = 0;
+
+		numerator = sensor->fi.interval.numerator;
+		denominator = sensor->fi.interval.denominator;
+		if (!numerator || !denominator) {
+			v4l2_err(&cif_dev->v4l2_dev,
+				 "get frame interval fail, numerator %d, denominator %d\n",
+				 numerator, denominator);
+			return -EINVAL;
+		}
+		def_fps = denominator / numerator;
+		if (!def_fps) {
+			v4l2_err(&cif_dev->v4l2_dev,
+				 "get fps fail, numerator %d, denominator %d\n",
+				 numerator, denominator);
+			return -EINVAL;
+		}
+		vblank_def = rkcif_get_sensor_vblank_def(cif_dev);
+		vblank_curr = rkcif_get_sensor_vblank(cif_dev);
+		if (!vblank_def || !vblank_curr) {
+			v4l2_err(&cif_dev->v4l2_dev,
+				 "get vblank fail, vblank_def %d, vblank_curr %d\n",
+				 vblank_def, vblank_curr);
+			return -EINVAL;
+		}
+		line_time = div_u64(1000000000, def_fps);
+		line_time = div_u64(line_time, vblank_def + sensor->raw_rect.height);
+		if (!line_time) {
+			v4l2_err(&cif_dev->v4l2_dev,
+				 "get line time fail, line_time %d\n",
+				 line_time);
+			return -EINVAL;
+		}
+		if (vblank_curr * line_time < 1000)
+			v4l2_warn(&cif_dev->v4l2_dev,
+				  "vblank < 1ms, val %d\n",
+				  vblank_curr * line_time);
+		cif_dev->isp_runtime_max = dbufs->runtime_us;
+		cif_dev->sensor_linetime = line_time;
+		cif_dev->early_line = div_u64(dbufs->runtime_us * 1000 - diff_time, line_time);
+		cif_dev->wait_line_cache = sensor->raw_rect.height - cif_dev->early_line;
+		if (cif_dev->rdbk_debug &&
+		    dbufs->sequence < 15)
+			v4l2_info(&cif_dev->v4l2_dev,
+				  "%s, isp runtime %d, line time %d, early_line %d, line_intr_cnt %d, seq %d, dma_addr %x\n",
+				  __func__, dbufs->runtime_us, line_time,
+				  cif_dev->early_line, cif_dev->wait_line_cache,
+				  dbufs->sequence, (u32)rx_buf->dummy.dma_addr);
+	} else {
+		if (dbufs->runtime_us > cif_dev->isp_runtime_max + cif_dev->sensor_linetime) {
+			cif_dev->isp_runtime_max = dbufs->runtime_us;
+			cif_dev->early_line = div_u64(dbufs->runtime_us * 1000 - diff_time, cif_dev->sensor_linetime);
+			cif_dev->wait_line_cache = sensor->raw_rect.height - cif_dev->early_line;
+		}
+		if (cif_dev->rdbk_debug &&
+		    dbufs->sequence < 15)
+			v4l2_info(&cif_dev->v4l2_dev,
+				  "isp runtime %d, seq %d, dma addr %x\n",
+				  dbufs->runtime_us, dbufs->sequence, (u32)rx_buf->dummy.dma_addr);
+	}
 	return 0;
 }