From 2b4692408b34bbbeb931ba619c314e87990d503a Mon Sep 17 00:00:00 2001 From: Zefa Chen Date: Mon, 4 Apr 2022 19:18:57 +0800 Subject: [PATCH] media: rockchip: vicap: rv1106 support set fps Signed-off-by: Zefa Chen Change-Id: Ia31e6ef65a638cb7860c15c8425a2e835d33acb1 --- drivers/media/platform/rockchip/cif/capture.c | 173 +++++++++++++++++- drivers/media/platform/rockchip/cif/dev.c | 55 ++++++ drivers/media/platform/rockchip/cif/dev.h | 10 + drivers/media/platform/rockchip/cif/regs.h | 5 + 4 files changed, 242 insertions(+), 1 deletion(-) diff --git a/drivers/media/platform/rockchip/cif/capture.c b/drivers/media/platform/rockchip/cif/capture.c index b46f641f4761..5b7e269e79bc 100644 --- a/drivers/media/platform/rockchip/cif/capture.c +++ b/drivers/media/platform/rockchip/cif/capture.c @@ -1368,6 +1368,28 @@ static void rkcif_s_rx_buffer(struct rkcif_device *dev, struct rkisp_rx_buf *dbu v4l2_subdev_call(sd, video, s_rx_buffer, dbufs, NULL); } +static void rkcif_enable_skip_frame(struct rkcif_stream *stream, int cap_m, int skip_n) +{ + struct rkcif_device *dev = stream->cifdev; + u32 val = 0; + + val = rkcif_read_register(dev, CIF_REG_MIPI_LVDS_CTRL); + val &= 0xc00fffff; + val |= cap_m << RKCIF_CAP_SHIFT | skip_n << RKCIF_SKIP_SHIFT | RKCIF_SKIP_EN(stream->id); + rkcif_write_register(dev, CIF_REG_MIPI_LVDS_CTRL, val); + stream->skip_info.skip_en = true; +} + +static void rkcif_disable_skip_frame(struct rkcif_stream *stream) +{ struct rkcif_device *dev = stream->cifdev; + u32 val = 0; + + val = rkcif_read_register(dev, CIF_REG_MIPI_LVDS_CTRL); + val &= ~(RKCIF_SKIP_EN(stream->id)); + rkcif_write_register(dev, CIF_REG_MIPI_LVDS_CTRL, val); + stream->skip_info.skip_en = false; +} + static void rkcif_assign_new_buffer_init_toisp(struct rkcif_stream *stream, int channel_id) { @@ -2440,6 +2462,19 @@ static int rkcif_lvds_get_output_type_mask(struct rkcif_stream *stream) return mask; } +static void rkcif_modify_frame_skip_config(struct rkcif_stream *stream) +{ + if (stream->skip_info.skip_to_en) { + rkcif_disable_skip_frame(stream); + rkcif_enable_skip_frame(stream, + stream->skip_info.cap_m, + stream->skip_info.skip_n); + stream->skip_info.skip_to_en = false; + } else if (stream->skip_info.skip_to_dis) { + rkcif_disable_skip_frame(stream); + } +} + /*config reg for rk3588*/ static int rkcif_csi_channel_set_v1(struct rkcif_stream *stream, struct csi_channel_info *channel, @@ -2576,7 +2611,8 @@ static int rkcif_csi_channel_set_v1(struct rkcif_stream *stream, val |= BIT(12); rkcif_write_register(dev, get_reg_index_of_lvds_id_ctrl0(channel->id), val); } - + if (dev->chip_id == CHIP_RV1106_CIF) + rkcif_modify_frame_skip_config(stream); stream->cifdev->id_use_cnt++; return 0; } @@ -3242,6 +3278,10 @@ void rkcif_do_stop_stream(struct rkcif_stream *stream, dev->is_start_hdr = false; stream->is_dvp_yuv_addr_init = false; + if (stream->skip_info.skip_en) { + stream->skip_info.skip_en = false; + stream->skip_info.skip_to_en = true; + } } else if (mode == RKCIF_STREAM_MODE_CAPTURE) { //only stop dma stream->to_stop_dma = RKCIF_DMAEN_BY_VICAP; @@ -3711,6 +3751,14 @@ int rkcif_update_sensor_info(struct rkcif_stream *stream) __func__, terminal_sensor->sd->name); return ret; } + ret = v4l2_subdev_call(terminal_sensor->sd, video, + g_frame_interval, &terminal_sensor->fi); + if (ret) { + v4l2_err(&stream->cifdev->v4l2_dev, + "%s: get terminal %s g_frame_interval failed!\n", + __func__, terminal_sensor->sd->name); + return ret; + } } else { v4l2_err(&stream->cifdev->v4l2_dev, "%s: stream[%d] get remote terminal sensor failed!\n", @@ -5070,6 +5118,122 @@ err: return -EINVAL; } +static int rkcif_get_max_common_div(int a, int b) +{ + int remainder = a % b; + + while (remainder != 0) { + a = b; + b = remainder; + remainder = a % b; + } + return b; +} + +void rkcif_set_fps(struct rkcif_stream *stream, struct rkcif_fps *fps) +{ + struct rkcif_sensor_info *sensor = &stream->cifdev->terminal_sensor; + struct rkcif_device *cif_dev = stream->cifdev; + struct rkcif_stream *tmp_stream = NULL; + struct v4l2_ctrl_handler *hdl = NULL; + struct v4l2_ctrl *ctrl = NULL; + u32 numerator, denominator; + u32 def_fps = 0; + u32 cur_fps = 0; + int cap_m, skip_n; + int i = 0; + int max_common_div; + bool skip_en = false; + bool is_get_vblank = false; + int ret = 0; + + if (!stream->cifdev->terminal_sensor.sd) { + ret = rkcif_update_sensor_info(stream); + if (ret) { + v4l2_err(&stream->cifdev->v4l2_dev, + "%s update sensor info fail\n", + __func__); + return; + } + + } + if (!stream->cifdev->terminal_sensor.sd) + return; + hdl = stream->cifdev->terminal_sensor.sd->ctrl_handler; + numerator = sensor->fi.interval.numerator; + denominator = sensor->fi.interval.denominator; + def_fps = denominator / numerator; + + if (!list_empty(&hdl->ctrls)) { + list_for_each_entry(ctrl, &hdl->ctrls, node) { + if (ctrl->id == V4L2_CID_VBLANK) { + is_get_vblank = true; + break; + } + } + } + if (is_get_vblank) + cur_fps = def_fps * (u32)(ctrl->default_value + stream->pixm.height) / + (u32)(ctrl->val + stream->pixm.height); + else + cur_fps = def_fps; + + if (fps->fps == 0 || fps->fps > cur_fps) { + v4l2_err(&stream->cifdev->v4l2_dev, + "set fps %d fps failed, current fps %d fps\n", + fps->fps, cur_fps); + return; + } + cap_m = fps->fps; + skip_n = cur_fps - fps->fps; + max_common_div = rkcif_get_max_common_div(cap_m, skip_n); + cap_m /= max_common_div; + skip_n /= max_common_div; + if (cap_m > 64) { + skip_n = skip_n / (cap_m / 64); + if (skip_n == 0) + skip_n = 1; + cap_m = 64; + } + if (skip_n > 7) { + cap_m = cap_m / (skip_n / 7); + if (cap_m == 0) + cap_m = 1; + skip_n = 7; + } + + if (fps->fps == cur_fps) + skip_en = false; + else + skip_en = true; + + if (fps->ch_num > 1 && fps->ch_num < 4) { + for (i = 0; i < fps->ch_num; i++) { + tmp_stream = &cif_dev->stream[i]; + if (skip_en) { + tmp_stream->skip_info.skip_to_en = true; + tmp_stream->skip_info.cap_m = cap_m; + tmp_stream->skip_info.skip_n = skip_n; + } else { + tmp_stream->skip_info.skip_to_dis = true; + } + } + } else { + if (skip_en) { + stream->skip_info.skip_to_en = true; + stream->skip_info.cap_m = cap_m; + stream->skip_info.skip_n = skip_n; + } else { + stream->skip_info.skip_to_dis = true; + } + } + v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev, + "skip_to_en %d, cap_m %d, skip_n %d\n", + stream->skip_info.skip_to_en, + cap_m, + skip_n); +} + static long rkcif_ioctl_default(struct file *file, void *fh, bool valid_prio, unsigned int cmd, void *arg) { @@ -5078,6 +5242,7 @@ static long rkcif_ioctl_default(struct file *file, void *fh, const struct cif_input_fmt *in_fmt; struct v4l2_rect rect; struct csi_channel_info csi_info; + struct rkcif_fps fps; switch (cmd) { case RKCIF_CMD_GET_CSI_MEMORY_MODE: @@ -5119,6 +5284,10 @@ static long rkcif_ioctl_default(struct file *file, void *fh, stream->is_high_align = false; } break; + case RKCIF_CMD_SET_FPS: + fps = *(struct rkcif_fps *)arg; + rkcif_set_fps(stream, &fps); + break; default: return -EINVAL; } @@ -7722,6 +7891,8 @@ void rkcif_irq_pingpong_v1(struct rkcif_device *cif_dev) is_update = rkcif_check_buffer_prepare(cif_dev); if (is_update) rkcif_update_stream(cif_dev, stream, mipi_id); + if (cif_dev->chip_id == CHIP_RV1106_CIF) + rkcif_modify_frame_skip_config(stream); } else if (stream->dma_en & RKCIF_DMAEN_BY_ISP) { rkcif_update_stream_toisp(cif_dev, stream, mipi_id); } diff --git a/drivers/media/platform/rockchip/cif/dev.c b/drivers/media/platform/rockchip/cif/dev.c index dfdccb0a010f..f28808002169 100644 --- a/drivers/media/platform/rockchip/cif/dev.c +++ b/drivers/media/platform/rockchip/cif/dev.c @@ -513,6 +513,60 @@ static ssize_t rkcif_store_scale_ch3_blc(struct device *dev, static DEVICE_ATTR(scale_ch3_blc, S_IWUSR | S_IRUSR, rkcif_show_scale_ch3_blc, rkcif_store_scale_ch3_blc); +static ssize_t rkcif_store_capture_fps(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct rkcif_device *cif_dev = (struct rkcif_device *)dev_get_drvdata(dev); + struct rkcif_stream *stream = NULL; + int i = 0, index = 0; + unsigned int val[4] = {0}; + unsigned int temp = 0; + int ret = 0; + int j = 0; + char cha[2] = {0}; + struct rkcif_fps fps = {0}; + + if (buf) { + index = 0; + for (i = 0; i < len; i++) { + if (((buf[i] == ' ') || (buf[i] == '\n')) && j) { + index++; + j = 0; + if (index == 4) + break; + continue; + } else { + if (buf[i] < '0' || buf[i] > '9') + continue; + cha[0] = buf[i]; + cha[1] = '\0'; + ret = kstrtoint(cha, 0, &temp); + if (!ret) { + if (j) + val[index] *= 10; + val[index] += temp; + j++; + } + } + } + + for (i = 0; i < index; i++) { + if ((val[i] - '0' != 0) && cif_dev->chip_id == CHIP_RV1106_CIF) { + stream = &cif_dev->stream[i]; + fps.fps = val[i]; + rkcif_set_fps(stream, &fps); + } + } + dev_info(cif_dev->dev, + "set fps id0: %d, id1: %d, id2: %d, id3: %d\n", + val[0], val[1], val[2], val[3]); + } + + return len; +} +static DEVICE_ATTR(fps, 0200, NULL, rkcif_store_capture_fps); + static struct attribute *dev_attrs[] = { &dev_attr_compact_test.attr, &dev_attr_wait_line.attr, @@ -522,6 +576,7 @@ static struct attribute *dev_attrs[] = { &dev_attr_scale_ch1_blc.attr, &dev_attr_scale_ch2_blc.attr, &dev_attr_scale_ch3_blc.attr, + &dev_attr_fps.attr, NULL, }; diff --git a/drivers/media/platform/rockchip/cif/dev.h b/drivers/media/platform/rockchip/cif/dev.h index 531dd46a39a1..26705a7c1980 100644 --- a/drivers/media/platform/rockchip/cif/dev.h +++ b/drivers/media/platform/rockchip/cif/dev.h @@ -437,6 +437,14 @@ enum rkcif_dma_en_mode { RKCIF_DMAEN_BY_ISP = 0x2, }; +struct rkcif_skip_info { + u8 cap_m; + u8 skip_n; + bool skip_en; + bool skip_to_en; + bool skip_to_dis; +}; + /* * struct rkcif_stream - Stream states TODO * @@ -491,6 +499,7 @@ struct rkcif_stream { int buf_num_toisp; u64 line_int_cnt; int lack_buf_cnt; + struct rkcif_skip_info skip_info; bool is_stop_dma; bool stopping; bool crop_enable; @@ -722,6 +731,7 @@ struct rkcif_device { }; extern struct platform_driver rkcif_plat_drv; +void rkcif_set_fps(struct rkcif_stream *stream, struct rkcif_fps *fps); int rkcif_do_start_stream(struct rkcif_stream *stream, enum rkcif_stream_mode mode); void rkcif_do_stop_stream(struct rkcif_stream *stream, diff --git a/drivers/media/platform/rockchip/cif/regs.h b/drivers/media/platform/rockchip/cif/regs.h index f4fc6b361d6b..7d60bd9a212e 100644 --- a/drivers/media/platform/rockchip/cif/regs.h +++ b/drivers/media/platform/rockchip/cif/regs.h @@ -1003,6 +1003,11 @@ enum cif_reg_index { #define SW_FRM_END_ID2(x) (((x) & CSI_FRAME_END_ID2) >> 12) #define SW_FRM_END_ID3(x) (((x) & CSI_FRAME_END_ID3) >> 14) +/*RV1106 SKIP FUNC*/ +#define RKCIF_CAP_SHIFT 0x18 +#define RKCIF_SKIP_SHIFT 0X15 +#define RKCIF_SKIP_EN(x) (0x1 << (8 + x)) + /* CIF LVDS SAV EAV Define */ #define SW_LVDS_EAV_ACT(code) (((code) & 0xfff) << 16) #define SW_LVDS_SAV_ACT(code) (((code) & 0xfff) << 0)