diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h index 4589a3b6cec6..020bd351bbd8 100644 --- a/drivers/clk/qcom/clk-rcg.h +++ b/drivers/clk/qcom/clk-rcg.h @@ -187,5 +187,6 @@ extern const struct clk_ops clk_byte2_ops; extern const struct clk_ops clk_pixel_ops; extern const struct clk_ops clk_gfx3d_ops; extern const struct clk_ops clk_gfx3d_src_ops; +extern const struct clk_ops clk_dp_ops; #endif diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c index 48ff5ea00040..4104a238c088 100644 --- a/drivers/clk/qcom/clk-rcg2.c +++ b/drivers/clk/qcom/clk-rcg2.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -847,6 +848,66 @@ const struct clk_ops clk_pixel_ops = { }; EXPORT_SYMBOL_GPL(clk_pixel_ops); +static int clk_dp_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct clk_rcg2 *rcg = to_clk_rcg2(hw); + struct freq_tbl f = { 0 }; + unsigned long src_rate; + unsigned long num, den; + u32 mask = BIT(rcg->hid_width) - 1; + u32 hid_div; + + src_rate = clk_get_rate(clk_hw_get_parent(hw)->clk); + if (src_rate <= 0) { + pr_err("Invalid RCG parent rate\n"); + return -EINVAL; + } + + rational_best_approximation(src_rate, rate, + (unsigned long)(1 << 16) - 1, + (unsigned long)(1 << 16) - 1, &den, &num); + + if (!num || !den) { + pr_err("Invalid MN values derived for requested rate %lu\n", + rate); + return -EINVAL; + } + + regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &hid_div); + f.pre_div = hid_div; + f.pre_div >>= CFG_SRC_DIV_SHIFT; + f.pre_div &= mask; + + if (num == den) { + f.m = 0; + f.n = 0; + } else { + f.m = num; + f.n = den; + } + + return clk_rcg2_configure(rcg, &f); +} + +static int clk_dp_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate, u8 index) +{ + return clk_dp_set_rate(hw, rate, parent_rate); +} + +const struct clk_ops clk_dp_ops = { + .is_enabled = clk_rcg2_is_enabled, + .get_parent = clk_rcg2_get_parent, + .set_parent = clk_rcg2_set_parent, + .recalc_rate = clk_rcg2_recalc_rate, + .set_rate = clk_dp_set_rate, + .set_rate_and_parent = clk_dp_set_rate_and_parent, + .determine_rate = clk_pixel_determine_rate, + .list_registers = clk_rcg2_list_registers, +}; +EXPORT_SYMBOL_GPL(clk_dp_ops); + static int clk_gfx3d_determine_rate(struct clk_hw *hw, struct clk_rate_request *req) {