diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c index cb8702aaef74..b1fb68752b5e 100644 --- a/drivers/clk/qcom/clk-rcg2.c +++ b/drivers/clk/qcom/clk-rcg2.c @@ -743,6 +743,45 @@ static void clk_rcg2_init(struct clk_hw *hw) rclk->ops = &clk_rcg2_regmap_ops; } +static int clk_rcg2_set_duty_cycle(struct clk_hw *hw, struct clk_duty *duty) +{ + struct clk_rcg2 *rcg = to_clk_rcg2(hw); + int ret; + u32 notn_m_val, n_val, m_val, d_val, not2d_val, mask; + u32 duty_per = (duty->num * 100) / duty->den; + + if (!rcg->mnd_width) + return 0; + + mask = BIT(rcg->mnd_width) - 1; + + regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m_val); + regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m_val); + + n_val = (~(notn_m_val) + m_val) & mask; + + /* Calculate 2d value */ + d_val = DIV_ROUND_CLOSEST(n_val * duty_per * 2, 100); + + /* Check BIT WIDTHS OF 2d. If D is too big reduce Duty cycle. */ + if (d_val > mask) + d_val = mask; + + if ((d_val / 2) > (n_val - m_val)) + d_val = (n_val - m_val) * 2; + else if ((d_val / 2) < (m_val / 2)) + d_val = m_val; + + not2d_val = ~d_val & mask; + + ret = regmap_update_bits(rcg->clkr.regmap, RCG_D_OFFSET(rcg), mask, + not2d_val); + if (ret) + return ret; + + return update_config(rcg); +} + const struct clk_ops clk_rcg2_ops = { .prepare = clk_prepare_regmap, .unprepare = clk_unprepare_regmap, @@ -757,6 +796,7 @@ const struct clk_ops clk_rcg2_ops = { .determine_rate = clk_rcg2_determine_rate, .set_rate = clk_rcg2_set_rate, .set_rate_and_parent = clk_rcg2_set_rate_and_parent, + .set_duty_cycle = clk_rcg2_set_duty_cycle, .init = clk_rcg2_init, .debug_init = clk_common_debug_init, #ifdef CONFIG_COMMON_CLK_QCOM_DEBUG