From 91ef5c438c1292e0d22942686bf268f1506937b7 Mon Sep 17 00:00:00 2001 From: Cai YiWei Date: Fri, 9 Dec 2022 15:57:22 +0800 Subject: [PATCH] media: rockchip: isp: dvfs for multi dev on/off Change-Id: I5ba04caebbaafd49d86bc615d1d69ea6ab2b9343 Signed-off-by: Cai YiWei --- drivers/media/platform/rockchip/isp/dev.c | 51 ++++++++++------- drivers/media/platform/rockchip/isp/dev.h | 2 +- drivers/media/platform/rockchip/isp/hw.h | 3 + drivers/media/platform/rockchip/isp/rkisp.c | 62 +++++++++++++++++---- 4 files changed, 87 insertions(+), 31 deletions(-) diff --git a/drivers/media/platform/rockchip/isp/dev.c b/drivers/media/platform/rockchip/isp/dev.c index 51efdfd457d2..a9c4d086dbfa 100644 --- a/drivers/media/platform/rockchip/isp/dev.c +++ b/drivers/media/platform/rockchip/isp/dev.c @@ -176,26 +176,34 @@ static int __isp_pipeline_s_isp_clk(struct rkisp_pipeline *p) { struct rkisp_device *dev = container_of(p, struct rkisp_device, pipe); struct rkisp_hw_dev *hw_dev = dev->hw_dev; - u32 w = hw_dev->max_in.w ? hw_dev->max_in.w : dev->isp_sdev.in_frm.width; struct v4l2_subdev *sd; struct v4l2_ctrl *ctrl; - u64 data_rate; - int i; + u64 data_rate = 0; + int i, fps; + + hw_dev->isp_size[dev->dev_id].is_on = true; + if (hw_dev->is_runing) { + if (dev->isp_ver >= ISP_V30 && !rkisp_clk_dbg) + hw_dev->is_dvfs = true; + return 0; + } if (dev->isp_inp & (INP_RAWRD0 | INP_RAWRD1 | INP_RAWRD2)) { - for (i = 0; i < hw_dev->num_clk_rate_tbl; i++) { - if (w <= hw_dev->clk_rate_tbl[i].refer_data) - break; + if (dev->isp_ver < ISP_V30) { + /* isp with mipi no support dvfs, calculate max data rate */ + for (i = 0; i < hw_dev->dev_num; i++) { + fps = hw_dev->isp_size[i].fps; + if (!fps) + fps = 30; + data_rate += (fps * hw_dev->isp_size[i].size); + } + } else { + i = dev->dev_id; + fps = hw_dev->isp_size[i].fps; + if (!fps) + fps = 30; + data_rate = fps * hw_dev->isp_size[i].size; } - if (!hw_dev->is_single) - i++; - - /* use lager clk in 4 vir-isp mode */ - if (hw_dev->dev_num >= 4) - i++; - - if (i > hw_dev->num_clk_rate_tbl - 1) - i = hw_dev->num_clk_rate_tbl - 1; goto end; } @@ -228,6 +236,7 @@ static int __isp_pipeline_s_isp_clk(struct rkisp_pipeline *p) data_rate = v4l2_ctrl_g_ctrl_int64(ctrl) * dev->isp_sdev.in_fmt.bus_width; data_rate >>= 3; +end: do_div(data_rate, 1000 * 1000); /* increase 25% margin */ @@ -239,7 +248,7 @@ static int __isp_pipeline_s_isp_clk(struct rkisp_pipeline *p) break; if (i == hw_dev->num_clk_rate_tbl) i--; -end: + /* set isp clock rate */ rkisp_set_clk_rate(hw_dev->clks[0], hw_dev->clk_rate_tbl[i].clk_rate * 1000000UL); if (hw_dev->is_unite) @@ -284,11 +293,13 @@ static int rkisp_pipeline_close(struct rkisp_pipeline *p) { struct rkisp_device *dev = container_of(p, struct rkisp_device, pipe); - atomic_dec(&p->power_cnt); - - if (!atomic_read(&p->power_cnt) && dev->isp_ver >= ISP_V30) - rkisp_rx_buf_pool_free(dev); + if (atomic_dec_return(&p->power_cnt)) + return 0; + rkisp_rx_buf_pool_free(dev); + dev->hw_dev->isp_size[dev->dev_id].is_on = false; + if (dev->hw_dev->is_runing && (dev->isp_ver >= ISP_V30) && !rkisp_clk_dbg) + dev->hw_dev->is_dvfs = true; return 0; } diff --git a/drivers/media/platform/rockchip/isp/dev.h b/drivers/media/platform/rockchip/isp/dev.h index a5fe217bcff8..37b52d159c49 100644 --- a/drivers/media/platform/rockchip/isp/dev.h +++ b/drivers/media/platform/rockchip/isp/dev.h @@ -227,7 +227,7 @@ struct rkisp_device { struct rkisp_ispp_buf *cur_fbcgain; struct rkisp_buffer *cur_spbuf; - struct tasklet_struct rdbk_tasklet; + struct work_struct rdbk_work; struct kfifo rdbk_kfifo; spinlock_t rdbk_lock; int rdbk_cnt; diff --git a/drivers/media/platform/rockchip/isp/hw.h b/drivers/media/platform/rockchip/isp/hw.h index cd689701e005..04dedc22921c 100644 --- a/drivers/media/platform/rockchip/isp/hw.h +++ b/drivers/media/platform/rockchip/isp/hw.h @@ -44,6 +44,8 @@ struct rkisp_size_info { u32 w; u32 h; u32 size; + u32 fps; + bool is_on; }; struct rkisp_hw_dev { @@ -101,6 +103,7 @@ struct rkisp_hw_dev { bool is_multi_overflow; bool is_runing; bool is_frm_buf; + bool is_dvfs; }; int rkisp_register_irq(struct rkisp_hw_dev *dev); diff --git a/drivers/media/platform/rockchip/isp/rkisp.c b/drivers/media/platform/rockchip/isp/rkisp.c index ed63881799c5..82bb3d09bf1c 100644 --- a/drivers/media/platform/rockchip/isp/rkisp.c +++ b/drivers/media/platform/rockchip/isp/rkisp.c @@ -425,7 +425,10 @@ int rkisp_update_sensor_info(struct rkisp_device *dev) v4l2_subdev_call(sensor->sd, video, g_frame_interval, &sensor->fi); dev->active_sensor = sensor; - + i = dev->dev_id; + if (sensor->fi.interval.numerator) + dev->hw_dev->isp_size[i].fps = + sensor->fi.interval.denominator / sensor->fi.interval.numerator; return ret; } @@ -492,6 +495,45 @@ u32 rkisp_mbus_pixelcode_to_v4l2(u32 pixelcode) return pixelformat; } +static void rkisp_dvfs(struct rkisp_device *dev) +{ + struct rkisp_hw_dev *hw = dev->hw_dev; + u64 data_rate = 0; + int i, fps, num = 0; + + if (!hw->is_dvfs) + return; + hw->is_dvfs = false; + for (i = 0; i < hw->dev_num; i++) { + if (!hw->isp_size[i].is_on) + continue; + fps = hw->isp_size[i].fps; + if (!fps) + fps = 30; + data_rate += (fps * hw->isp_size[i].size); + num++; + } + do_div(data_rate, 1000 * 1000); + /* increase margin: 25% * num */ + data_rate += (data_rate >> 2) * num; + + /* compare with isp clock adjustment table */ + for (i = 0; i < hw->num_clk_rate_tbl; i++) + if (data_rate <= hw->clk_rate_tbl[i].clk_rate) + break; + if (i == hw->num_clk_rate_tbl) + i--; + + /* set isp clock rate */ + rkisp_set_clk_rate(hw->clks[0], hw->clk_rate_tbl[i].clk_rate * 1000000UL); + if (hw->is_unite) + rkisp_set_clk_rate(hw->clks[5], hw->clk_rate_tbl[i].clk_rate * 1000000UL); + /* aclk equal to core clk */ + if (dev->isp_ver == ISP_V32) + rkisp_set_clk_rate(hw->clks[1], hw->clk_rate_tbl[i].clk_rate * 1000000UL); + dev_info(hw->dev, "set isp clk = %luHz\n", clk_get_rate(hw->clks[0])); +} + static void rkisp_multi_overflow_hdl(struct rkisp_device *dev, bool on) { struct rkisp_hw_dev *hw = dev->hw_dev; @@ -779,7 +821,6 @@ static void rkisp_fast_switch_rx_buf(struct rkisp_device *dev, bool is_current) } } - static void rkisp_rdbk_trigger_handle(struct rkisp_device *dev, u32 cmd) { struct rkisp_hw_dev *hw = dev->hw_dev; @@ -902,10 +943,11 @@ int rkisp_rdbk_trigger_event(struct rkisp_device *dev, u32 cmd, void *arg) return ret; } -static void rkisp_rdbk_task(unsigned long arg) +static void rkisp_rdbk_work(struct work_struct *work) { - struct rkisp_device *dev = (struct rkisp_device *)arg; + struct rkisp_device *dev = container_of(work, struct rkisp_device, rdbk_work); + rkisp_dvfs(dev); rkisp_rdbk_trigger_event(dev, T_CMD_END, NULL); } @@ -971,7 +1013,10 @@ void rkisp_check_idle(struct rkisp_device *dev, u32 irq) end: dev->irq_ends = 0; - tasklet_schedule(&dev->rdbk_tasklet); + if (dev->hw_dev->is_dvfs) + schedule_work(&dev->rdbk_work); + else + rkisp_rdbk_trigger_event(dev, T_CMD_END, NULL); } static void rkisp_set_state(u32 *state, u32 val) @@ -2033,6 +2078,7 @@ static int rkisp_isp_stop(struct rkisp_device *dev) rkisp_next_write(dev, CSI2RX_CSI2_RESETN, 0, true); } + hw->is_dvfs = false; hw->is_runing = false; dev->hw_dev->is_idle = true; dev->hw_dev->is_mi_update = false; @@ -2789,12 +2835,10 @@ static int rkisp_isp_sd_s_stream(struct v4l2_subdev *sd, int on) atomic_dec(&hw_dev->refcnt); rkisp_params_stream_stop(&isp_dev->params_vdev); atomic_set(&isp_dev->isp_sdev.frm_sync_seq, 0); - tasklet_disable(&isp_dev->rdbk_tasklet); return 0; } hw_dev->is_runing = true; - tasklet_enable(&isp_dev->rdbk_tasklet); rkisp_start_3a_run(isp_dev); memset(&isp_dev->isp_sdev.dbg, 0, sizeof(isp_dev->isp_sdev.dbg)); if (atomic_inc_return(&hw_dev->refcnt) > hw_dev->dev_link_num) { @@ -3670,8 +3714,7 @@ int rkisp_register_isp_subdev(struct rkisp_device *isp_dev, isp_dev->isp_state = ISP_STOP; atomic_set(&isp_sdev->frm_sync_seq, 0); rkisp_monitor_init(isp_dev); - tasklet_init(&isp_dev->rdbk_tasklet, rkisp_rdbk_task, (unsigned long)isp_dev); - tasklet_disable(&isp_dev->rdbk_tasklet); + INIT_WORK(&isp_dev->rdbk_work, rkisp_rdbk_work); return 0; err_cleanup_media_entity: media_entity_cleanup(&sd->entity); @@ -3684,7 +3727,6 @@ void rkisp_unregister_isp_subdev(struct rkisp_device *isp_dev) { struct v4l2_subdev *sd = &isp_dev->isp_sdev.sd; - tasklet_kill(&isp_dev->rdbk_tasklet); kfifo_free(&isp_dev->rdbk_kfifo); v4l2_device_unregister_subdev(sd); media_entity_cleanup(&sd->entity);