diff --git a/drivers/media/platform/rockchip/fec/fec_offline.c b/drivers/media/platform/rockchip/fec/fec_offline.c index d8a95ad0e686..4dac639aaa77 100644 --- a/drivers/media/platform/rockchip/fec/fec_offline.c +++ b/drivers/media/platform/rockchip/fec/fec_offline.c @@ -21,6 +21,37 @@ MODULE_PARM_DESC(debug, "Debug level (0-6)"); static void buf_del(struct file *file, int fd, bool is_all); +static void rkfec_dvfs(struct rkfec_offline_dev *ofl, int width) +{ + int i, ret; + struct rkfec_hw_dev *hw = ofl->hw; + const struct fec_clk_info *rate_info = NULL; + unsigned long target_rate = 0; + + for (i = 0; i < hw->clk_rate_tbl_num; i++) { + if (width <= hw->clk_rate_tbl[i].refer_data) { + rate_info = &hw->clk_rate_tbl[i]; + break; + } + } + + if (!rate_info) + rate_info = &hw->clk_rate_tbl[hw->clk_rate_tbl_num - 1]; + + target_rate = rate_info->clk_rate * 1000000; + + ret = hw->set_clk(hw->clks[0], target_rate); + if (ret < 0) + v4l2_err(&ofl->v4l2_dev, "failed to set aclk rate: %d\n", ret); + + ret = hw->set_clk(hw->clks[2], target_rate); + if (ret < 0) + v4l2_err(&ofl->v4l2_dev, "failed to set core clk rate: %d\n", ret); + + v4l2_dbg(4, rkfec_debug, &ofl->v4l2_dev, "set clk rate: %ld\n", + target_rate); +} + #if IS_LINUX_VERSION_AT_LEAST_6_1 static void init_vb2(struct rkfec_offline_dev *ofl, struct rkfec_offline_buf *buf) @@ -257,6 +288,9 @@ static int fec_running(struct file *file, struct rkfec_in_out *buf) dev_warn(hw->dev, "soft_reset not implemented\n"); } + if (hw->set_clk) + rkfec_dvfs(ofl, in_w); + init_completion(&ofl->cmpl); switch (buf->in_fourcc) { diff --git a/drivers/media/platform/rockchip/fec/hw.c b/drivers/media/platform/rockchip/fec/hw.c index dbf9eb0d4844..342a1decdee9 100644 --- a/drivers/media/platform/rockchip/fec/hw.c +++ b/drivers/media/platform/rockchip/fec/hw.c @@ -22,12 +22,12 @@ static const char * const rv1126b_fec_clks[] = { "clk_fec", }; -static void rkfec_set_clk_rate(struct clk *clk, unsigned long rate) +static int rkfec_set_clk_rate(struct clk *clk, unsigned long rate) { if (rkfec_clk_dbg) - return; + return 0; - clk_set_rate(clk, rate); + return clk_set_rate(clk, rate); } static void rkfec_soft_reset(struct rkfec_hw_dev *hw) @@ -92,9 +92,6 @@ static int enable_sys_clk(struct rkfec_hw_dev *dev) goto err; } - rkfec_set_clk_rate(dev->clks[0], - dev->clk_rate_tbl[dev->clk_rate_tbl_num - 1].clk_rate * 1000000); - return 0; err: @@ -287,6 +284,7 @@ static int rkfec_hw_probe(struct platform_device *pdev) hw_dev->is_dma_config = false; hw_dev->mem_ops = &vb2_cma_sg_memops; hw_dev->soft_reset = rkfec_soft_reset; + hw_dev->set_clk = rkfec_set_clk_rate; rkfec_register_offline(hw_dev); diff --git a/drivers/media/platform/rockchip/fec/hw.h b/drivers/media/platform/rockchip/fec/hw.h index f131003e8aef..379cb6f679c0 100644 --- a/drivers/media/platform/rockchip/fec/hw.h +++ b/drivers/media/platform/rockchip/fec/hw.h @@ -73,6 +73,7 @@ struct rkfec_hw_dev { enum rkfec_fec_ver fec_ver; void (*soft_reset)(struct rkfec_hw_dev *hw); + int (*set_clk)(struct clk *clk, unsigned long rate); }; #ifndef IS_LINUX_VERSION_AT_LEAST_6_1