From b0127db2921ead712dbed9d3f0430933a8e9572e Mon Sep 17 00:00:00 2001 From: Lian Xu Date: Thu, 24 Mar 2022 15:29:21 +0800 Subject: [PATCH] media: rockchip: isp: ctrl the fps for isp32 Change-Id: Ia2d780d263bc14327527b9e87607a823f1e7936f Signed-off-by: Lian Xu --- drivers/media/platform/rockchip/isp/capture.c | 28 +++++ .../media/platform/rockchip/isp/capture_v32.c | 6 +- .../media/platform/rockchip/isp/capture_v3x.h | 6 + .../media/platform/rockchip/isp/isp_rockit.c | 119 +++++++++++++++++- include/soc/rockchip/rockchip_rockit.h | 6 + include/uapi/linux/rkisp2-config.h | 5 + 6 files changed, 166 insertions(+), 4 deletions(-) diff --git a/drivers/media/platform/rockchip/isp/capture.c b/drivers/media/platform/rockchip/isp/capture.c index 928c42e03cc6..90e471552edb 100644 --- a/drivers/media/platform/rockchip/isp/capture.c +++ b/drivers/media/platform/rockchip/isp/capture.c @@ -1457,6 +1457,28 @@ static int rkisp_set_wrap_line(struct rkisp_stream *stream, int *line) return 0; } +static int rkisp_set_fps(struct rkisp_stream *stream, int *fps) +{ + struct rkisp_device *dev = stream->ispdev; + + if (dev->isp_ver != ISP_V32) + return -EINVAL; + + rkisp_rockit_fps_set(fps, stream->id); + return 0; +} + +static int rkisp_get_fps(struct rkisp_stream *stream, int *fps) +{ + struct rkisp_device *dev = stream->ispdev; + + if (dev->isp_ver != ISP_V32) + return -EINVAL; + + rkisp_rockit_fps_get(fps, stream->id); + return 0; +} + static long rkisp_ioctl_default(struct file *file, void *fh, bool valid_prio, unsigned int cmd, void *arg) { @@ -1515,6 +1537,12 @@ static long rkisp_ioctl_default(struct file *file, void *fh, case RKISP_CMD_SET_WRAP_LINE: ret = rkisp_set_wrap_line(stream, arg); break; + case RKISP_CMD_SET_FPS: + ret = rkisp_set_fps(stream, arg); + break; + case RKISP_CMD_GET_FPS: + ret = rkisp_get_fps(stream, arg); + break; default: ret = -EINVAL; } diff --git a/drivers/media/platform/rockchip/isp/capture_v32.c b/drivers/media/platform/rockchip/isp/capture_v32.c index ad93d2eec5dc..1c27eb2a8dbc 100644 --- a/drivers/media/platform/rockchip/isp/capture_v32.c +++ b/drivers/media/platform/rockchip/isp/capture_v32.c @@ -992,10 +992,10 @@ static int mi_frame_start(struct rkisp_stream *stream, u32 mis) { unsigned long lock_flags = 0; - if (stream->ispdev->cap_dev.wrap_line && - stream->id == RKISP_STREAM_MP && - stream->streaming && mis != 0) + if (mis && stream->streaming) { rkisp_rockit_buf_done(stream, ROCKIT_DVBM_START); + rkisp_rockit_ctrl_fps(stream); + } /* readback start to update stream buf if null */ spin_lock_irqsave(&stream->vbq_lock, lock_flags); diff --git a/drivers/media/platform/rockchip/isp/capture_v3x.h b/drivers/media/platform/rockchip/isp/capture_v3x.h index f0c686da58a1..c72eb60af6f8 100644 --- a/drivers/media/platform/rockchip/isp/capture_v3x.h +++ b/drivers/media/platform/rockchip/isp/capture_v3x.h @@ -28,6 +28,9 @@ void rkisp_mipi_v32_isr(u32 phy, u32 packet, u32 overflow, u32 state, struct rki int rkisp_rockit_buf_free(struct rkisp_stream *stream); void rkisp_rockit_dev_init(struct rkisp_device *dev); +bool rkisp_rockit_ctrl_fps(struct rkisp_stream *stream); +void rkisp_rockit_fps_set(int *dst_fps, unsigned int id); +void rkisp_rockit_fps_get(int *dst_fps, unsigned int id); int rkisp_rockit_buf_done(struct rkisp_stream *stream, int cmd); #else static inline int rkisp_register_stream_v32(struct rkisp_device *dev) { return -EINVAL; } @@ -37,6 +40,9 @@ static inline void rkisp_mipi_v32_isr(u32 phy, u32 packet, u32 overflow, u32 sta static inline int rkisp_rockit_buf_free(struct rkisp_stream *stream) { return -EINVAL; } static inline void rkisp_rockit_dev_init(struct rkisp_device *dev) { return; } +static inline bool rkisp_rockit_ctrl_fps(struct rkisp_stream *stream) { return false; } +static inline void rkisp_rockit_fps_set(int *dst_fps, unsigned int id) { return; } +static inline void rkisp_rockit_fps_get(int *dst_fps, unsigned int id) { return; } static inline int rkisp_rockit_buf_done(struct rkisp_stream *stream, int cmd) { return -EINVAL; } #endif diff --git a/drivers/media/platform/rockchip/isp/isp_rockit.c b/drivers/media/platform/rockchip/isp/isp_rockit.c index db7498888f83..d05c97e45b92 100644 --- a/drivers/media/platform/rockchip/isp/isp_rockit.c +++ b/drivers/media/platform/rockchip/isp/isp_rockit.c @@ -178,6 +178,9 @@ int rkisp_rockit_buf_queue(struct rockit_cfg *input_rockit_cfg) "stream:%d rockit_queue buf:0x%x\n", stream->id, isprk_buf->isp_buf.buff_addr[0]); + if (stream_cfg->is_discard && stream->streaming) + return -EINVAL; + spin_lock_irqsave(&stream->vbq_lock, lock_flags); /* single sensor with pingpong buf, update next if need */ if (stream->ispdev->hw_dev->is_single && @@ -207,7 +210,7 @@ int rkisp_rockit_buf_done(struct rkisp_stream *stream, int cmd) return -EINVAL; } - if (stream->curr_buf != NULL) { + if (cmd == ROCKIT_DVBM_END) { isprk_buf = container_of(stream->curr_buf, struct rkisp_rockit_buffer, isp_buf); @@ -217,6 +220,21 @@ int rkisp_rockit_buf_done(struct rkisp_stream *stream, int cmd) rockit_cfg->frame.u32TimeRef = stream->curr_buf->vb.sequence; } else { + if (stream->ispdev->cap_dev.wrap_line && + stream->id == RKISP_STREAM_MP) { + if (stream_cfg->is_discard) + return 0; + } else if (stream_cfg->dst_fps) { + if (!stream_cfg->is_discard && !stream->curr_buf) { + rockit_cfg->is_qbuf = true; + } else { + rockit_cfg->is_qbuf = false; + return 0; + } + } else { + return 0; + } + rkisp_dmarx_get_frame(stream->ispdev, &seq, NULL, &ns, true); if (!ns) @@ -261,6 +279,8 @@ int rkisp_rockit_buf_free(struct rkisp_stream *stream) const struct vb2_mem_ops *g_ops = stream->ispdev->hw_dev->mem_ops; struct rkisp_stream_cfg *stream_cfg = &rockit_cfg->rkisp_stream_cfg[stream->id]; + stream_cfg->is_discard = false; + for (i = 0; i < ROCKIT_BUF_NUM_MAX; i++) { if (stream_cfg->rkisp_buff[i]) { isprk_buf = (struct rkisp_rockit_buffer *)stream_cfg->rkisp_buff[i]; @@ -297,6 +317,103 @@ void rkisp_rockit_dev_init(struct rkisp_device *dev) } } +void rkisp_rockit_fps_set(int *dst_fps, unsigned int id) +{ + if (id >= ROCKIT_STREAM_NUM_MAX) { + pr_err("fps_set stream id %u exceeds maximum\n", id); + return; + } + + if (dst_fps == NULL) { + pr_err("fps_set dst_fps is null\n"); + return; + } + + rockit_cfg->rkisp_stream_cfg[id].dst_fps = *dst_fps; + rockit_cfg->rkisp_stream_cfg[id].fps_cnt = *dst_fps; +} + +void rkisp_rockit_fps_get(int *dst_fps, unsigned int id) +{ + if (id >= ROCKIT_STREAM_NUM_MAX) { + pr_err("fps_get stream id %u exceeds maximum\n", id); + return; + } + + if (dst_fps == NULL) { + pr_err("fps_get dst_fps is null\n"); + return; + } + + *dst_fps = rockit_cfg->rkisp_stream_cfg[id].cur_fps; +} + +bool rkisp_rockit_ctrl_fps(struct rkisp_stream *stream) +{ + struct rkisp_device *dev = stream->ispdev; + struct rkisp_sensor_info *sensor = NULL; + int *fps_cnt = &rockit_cfg->rkisp_stream_cfg[stream->id].fps_cnt; + int ret, dst_fps; + static int fps_in, cur_fps[ROCKIT_STREAM_NUM_MAX]; + u32 denominator = 0, numerator = 0; + bool *is_discard = &rockit_cfg->rkisp_stream_cfg[stream->id].is_discard; + u64 cur_time, *old_time = &rockit_cfg->rkisp_stream_cfg[stream->id].old_time; + + dst_fps = rockit_cfg->rkisp_stream_cfg[stream->id].dst_fps; + if (dst_fps == 0 || !stream->streaming) { + *is_discard = false; + return false; + } + + if (dev->active_sensor == NULL) { + *is_discard = false; + pr_err("the sensor is not found\n"); + return false; + } + + sensor = dev->active_sensor; + + ret = v4l2_subdev_call(sensor->sd, video, g_frame_interval, &sensor->fi); + if (!ret) { + denominator = sensor->fi.interval.denominator; + numerator = sensor->fi.interval.numerator; + if (numerator) + fps_in = denominator / numerator; + else { + *is_discard = false; + pr_err("the numerator is 0\n"); + return false; + } + } + + if (dst_fps >= fps_in) + return false; + + if ((fps_in > 0) && (dst_fps > 0)) { + if (*fps_cnt < 0) + *fps_cnt = fps_in - dst_fps; + *fps_cnt += dst_fps; + + if (*fps_cnt < fps_in) + *is_discard = true; + else { + *fps_cnt -= fps_in; + *is_discard = false; + ++cur_fps[stream->id]; + cur_time = ktime_get_ns(); + if (cur_time - *old_time >= 1000000000) { + *old_time = cur_time; + rockit_cfg->rkisp_stream_cfg[stream->id].cur_fps = + cur_fps[stream->id]; + cur_fps[stream->id] = 0; + } + } + } else { + *is_discard = false; + } + return true; +} + void *rkisp_rockit_function_register(void *function, int cmd) { if (rockit_cfg == NULL) { diff --git a/include/soc/rockchip/rockchip_rockit.h b/include/soc/rockchip/rockchip_rockit.h index 9b688f86bb6a..56857c781b6b 100644 --- a/include/soc/rockchip/rockchip_rockit.h +++ b/include/soc/rockchip/rockchip_rockit.h @@ -20,6 +20,11 @@ struct rkisp_stream_cfg { struct rkisp_rockit_buffer *rkisp_buff[ROCKIT_BUF_NUM_MAX]; int buff_id[ROCKIT_BUF_NUM_MAX]; void *node; + int fps_cnt; + int dst_fps; + int cur_fps; + u64 old_time; + bool is_discard; }; struct ISP_VIDEO_FRAMES { @@ -49,6 +54,7 @@ struct rkisp_dev_cfg { struct rockit_cfg { bool is_alloc; bool is_empty; + bool is_qbuf; char *current_name; dma_addr_t dma_addr; int *buff_id; diff --git a/include/uapi/linux/rkisp2-config.h b/include/uapi/linux/rkisp2-config.h index 7b6e7dabf222..a7bd1c2c00e6 100644 --- a/include/uapi/linux/rkisp2-config.h +++ b/include/uapi/linux/rkisp2-config.h @@ -79,6 +79,11 @@ #define RKISP_CMD_SET_WRAP_LINE \ _IOW('V', BASE_VIDIOC_PRIVATE + 108, int) +#define RKISP_CMD_SET_FPS \ + _IOW('V', BASE_VIDIOC_PRIVATE + 109, int) + +#define RKISP_CMD_GET_FPS \ + _IOR('V', BASE_VIDIOC_PRIVATE + 110, int) /*************************************************************/ #define ISP2X_ID_DPCC (0)