diff --git a/drivers/media/platform/rockchip/isp1/dev.c b/drivers/media/platform/rockchip/isp1/dev.c index cf7bf0341269..aa50ea969971 100644 --- a/drivers/media/platform/rockchip/isp1/dev.c +++ b/drivers/media/platform/rockchip/isp1/dev.c @@ -51,8 +51,10 @@ struct isp_match_data { const char * const *clks; - int size; + int num_clks; enum rkisp1_isp_ver isp_ver; + const unsigned int *clk_rate_tbl; + int num_clk_rate_tbl; }; int rkisp1_debug; @@ -140,6 +142,54 @@ static int __isp_pipeline_s_power(struct rkisp1_pipeline *p, bool on) return ret; } +static int __isp_pipeline_s_isp_clk(struct rkisp1_pipeline *p) +{ + struct rkisp1_device *dev = container_of(p, struct rkisp1_device, pipe); + struct v4l2_subdev *sd; + struct v4l2_ctrl *ctrl; + u64 data_rate; + int i; + + /* find the subdev of active sensor */ + sd = p->subdevs[0]; + for (i = 0; i < p->num_subdevs; i++) { + sd = p->subdevs[i]; + if (sd->entity.type == MEDIA_ENT_T_V4L2_SUBDEV_SENSOR) + break; + } + + if (i == p->num_subdevs) { + v4l2_warn(sd, "No active sensor\n"); + return -EPIPE; + } + + ctrl = v4l2_ctrl_find(sd->ctrl_handler, V4L2_CID_PIXEL_RATE); + if (!ctrl) { + v4l2_warn(sd, "No pixel rate control in subdev\n"); + return -EPIPE; + } + + /* calculate data rate */ + data_rate = v4l2_ctrl_g_ctrl_int64(ctrl) * + dev->isp_sdev.in_fmt.bus_width; + data_rate >>= 3; + do_div(data_rate, 1000 * 1000); + + /* compare with isp clock adjustment table */ + for (i = 0; i < dev->num_clk_rate_tbl; i++) + if (data_rate <= dev->clk_rate_tbl[i]) + break; + if (i == dev->num_clk_rate_tbl) + i--; + + /* set isp clock rate */ + clk_set_rate(dev->clks[0], dev->clk_rate_tbl[i] * 1000000UL); + v4l2_dbg(1, rkisp1_debug, sd, "set isp clk = %luHz\n", + clk_get_rate(dev->clks[0])); + + return 0; +} + static int rkisp1_pipeline_open(struct rkisp1_pipeline *p, struct media_entity *me, bool prepare) @@ -158,6 +208,10 @@ static int rkisp1_pipeline_open(struct rkisp1_pipeline *p, if (!p->num_subdevs) return -EINVAL; + ret = __isp_pipeline_s_isp_clk(p); + if (ret < 0) + return ret; + ret = __isp_pipeline_s_power(p, 1); if (ret < 0) return ret; @@ -454,22 +508,45 @@ static const char * const rk3399_isp_clks[] = { "pclk_isp_wrap" }; +/* isp clock adjustment table (MHz) */ +/* TODO: test on rk3288 */ +static const unsigned int rk3288_isp_clk_rate[] = { + 300, 400, 600 +}; + +/* isp clock adjustment table (MHz) */ +/* TODO: test on rk3326 */ +static const unsigned int rk3326_isp_clk_rate[] = { + 300, 400, 600 +}; + +/* isp clock adjustment table (MHz) */ +static const unsigned int rk3399_isp_clk_rate[] = { + 300, 400, 600 +}; + static const struct isp_match_data rk3288_isp_match_data = { .clks = rk3288_isp_clks, - .size = ARRAY_SIZE(rk3288_isp_clks), + .num_clks = ARRAY_SIZE(rk3288_isp_clks), .isp_ver = ISP_V10, + .clk_rate_tbl = rk3288_isp_clk_rate, + .num_clk_rate_tbl = ARRAY_SIZE(rk3288_isp_clk_rate), }; static const struct isp_match_data rk3326_isp_match_data = { .clks = rk3326_isp_clks, - .size = ARRAY_SIZE(rk3326_isp_clks), + .num_clks = ARRAY_SIZE(rk3326_isp_clks), .isp_ver = ISP_V12, + .clk_rate_tbl = rk3326_isp_clk_rate, + .num_clk_rate_tbl = ARRAY_SIZE(rk3326_isp_clk_rate), }; static const struct isp_match_data rk3399_isp_match_data = { .clks = rk3399_isp_clks, - .size = ARRAY_SIZE(rk3399_isp_clks), + .num_clks = ARRAY_SIZE(rk3399_isp_clks), .isp_ver = ISP_V10, + .clk_rate_tbl = rk3399_isp_clk_rate, + .num_clk_rate_tbl = ARRAY_SIZE(rk3399_isp_clk_rate), }; static const struct of_device_id rkisp1_plat_of_match[] = { @@ -511,7 +588,7 @@ static void rkisp1_disable_sys_clk(struct rkisp1_device *rkisp1_dev) { int i; - for (i = rkisp1_dev->clk_size - 1; i >= 0; i--) + for (i = rkisp1_dev->num_clks - 1; i >= 0; i--) if (!IS_ERR(rkisp1_dev->clks[i])) clk_disable_unprepare(rkisp1_dev->clks[i]); } @@ -520,7 +597,7 @@ static int rkisp1_enable_sys_clk(struct rkisp1_device *rkisp1_dev) { int i, ret = -EINVAL; - for (i = 0; i < rkisp1_dev->clk_size; i++) { + for (i = 0; i < rkisp1_dev->num_clks; i++) { if (!IS_ERR(rkisp1_dev->clks[i])) { ret = clk_prepare_enable(rkisp1_dev->clks[i]); if (ret < 0) @@ -623,15 +700,17 @@ static int rkisp1_plat_probe(struct platform_device *pdev) isp_dev->irq = irq; match_data = match->data; - for (i = 0; i < match_data->size; i++) { + for (i = 0; i < match_data->num_clks; i++) { struct clk *clk = devm_clk_get(dev, match_data->clks[i]); if (IS_ERR(clk)) dev_dbg(dev, "failed to get %s\n", match_data->clks[i]); isp_dev->clks[i] = clk; } - isp_dev->clk_size = match_data->size; + isp_dev->num_clks = match_data->num_clks; isp_dev->isp_ver = match_data->isp_ver; + isp_dev->clk_rate_tbl = match_data->clk_rate_tbl; + isp_dev->num_clk_rate_tbl = match_data->num_clk_rate_tbl; atomic_set(&isp_dev->pipe.power_cnt, 0); atomic_set(&isp_dev->pipe.stream_cnt, 0); diff --git a/drivers/media/platform/rockchip/isp1/dev.h b/drivers/media/platform/rockchip/isp1/dev.h index 9f3b0fff2978..36b2eac5cea9 100644 --- a/drivers/media/platform/rockchip/isp1/dev.h +++ b/drivers/media/platform/rockchip/isp1/dev.h @@ -106,7 +106,7 @@ struct rkisp1_device { int irq; struct device *dev; struct clk *clks[RKISP1_MAX_BUS_CLK]; - int clk_size; + int num_clks; struct v4l2_device v4l2_dev; struct v4l2_ctrl_handler ctrl_handler; struct media_device media_dev; @@ -123,6 +123,8 @@ struct rkisp1_device { struct vb2_alloc_ctx *alloc_ctx; struct iommu_domain *domain; enum rkisp1_isp_ver isp_ver; + const unsigned int *clk_rate_tbl; + int num_clk_rate_tbl; }; #endif