From 6f16847710cc0502450788b9f12f0a14d3429668 Mon Sep 17 00:00:00 2001 From: Phil Elwell Date: Wed, 6 Mar 2024 14:44:04 +0000 Subject: [PATCH 0934/1085] clk: rp1: Allow clk_i2s to change the audio PLLs Add dedicated code allowing the audio PLLs to be changed, enabling perfect I2S clock generation. The slowest legal pll_audio_core and pll_audio will be selected that leads to the required clk_i2s rate. Signed-off-by: Phil Elwell --- drivers/clk/clk-rp1.c | 115 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 114 insertions(+), 1 deletion(-) --- a/drivers/clk/clk-rp1.c +++ b/drivers/clk/clk-rp1.c @@ -254,6 +254,7 @@ const char * const fc0_ref_clk_name = "clk_slow_sys"; #define ABS_DIFF(a, b) ((a) > (b) ? (a) - (b) : (b) - (a)) +#define DIV_NEAREST(a, b) (((a) + ((b) >> 1)) / (b)) #define DIV_U64_NEAREST(a, b) div_u64(((a) + ((b) >> 1)), (b)) /* @@ -393,6 +394,18 @@ struct rp1_clock { unsigned long cached_rate; }; + +struct rp1_clk_change { + struct clk_hw *hw; + unsigned long new_rate; +}; + +struct rp1_clk_change rp1_clk_chg_tree[3]; + +static struct clk_hw *clk_xosc; +static struct clk_hw *clk_audio; +static struct clk_hw *clk_i2s; + static void rp1_debugfs_regset(struct rp1_clockman *clockman, u32 base, const struct debugfs_reg32 *regs, size_t nregs, struct dentry *dentry) @@ -749,8 +762,12 @@ static unsigned long rp1_pll_recalc_rate static long rp1_pll_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *parent_rate) { + const struct rp1_clk_change *chg = &rp1_clk_chg_tree[1]; u32 div1, div2; + if (chg->hw == hw && chg->new_rate == rate) + *parent_rate = chg[1].new_rate; + get_pll_prim_dividers(rate, *parent_rate, &div1, &div2); return DIV_ROUND_CLOSEST(*parent_rate, div1 * div2); @@ -1188,6 +1205,59 @@ static int rp1_clock_set_rate(struct clk return rp1_clock_set_rate_and_parent(hw, rate, parent_rate, 0xff); } +static unsigned long calc_core_pll_rate(struct clk_hw *pll_hw, + unsigned long target_rate, + int *pdiv_prim, int *pdiv_clk) +{ + static const int prim_divs[] = { + 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 14, 15, 16, + 18, 20, 21, 24, 25, 28, 30, 35, 36, 42, 49, + }; + const unsigned long xosc_rate = clk_hw_get_rate(clk_xosc); + const unsigned long core_max = 2400000000; + const unsigned long core_min = xosc_rate * 16; + unsigned long best_rate = core_max + 1; + int best_div_prim = 1, best_div_clk = 1; + unsigned long core_rate = 0; + int div_int, div_frac; + u64 div; + int i; + + /* Given the target rate, choose a set of divisors/multipliers */ + for (i = 0; i < ARRAY_SIZE(prim_divs); i++) { + int div_prim = prim_divs[i]; + int div_clk; + + for (div_clk = 1; div_clk <= 256; div_clk++) { + core_rate = target_rate * div_clk * div_prim; + if (core_rate >= core_min) { + if (core_rate < best_rate) { + best_rate = core_rate; + best_div_prim = div_prim; + best_div_clk = div_clk; + } + break; + } + } + } + + if (best_rate < core_max) { + div = ((best_rate << 24) + xosc_rate / 2) / xosc_rate; + div_int = div >> 24; + div_frac = div % (1 << 24); + core_rate = (xosc_rate * ((div_int << 24) + div_frac) + (1 << 23)) >> 24; + } else { + core_rate = 0; + } + + if (pdiv_prim) + *pdiv_prim = best_div_prim; + if (pdiv_clk) + *pdiv_clk = best_div_clk; + + return core_rate; +} + static void rp1_clock_choose_div_and_prate(struct clk_hw *hw, int parent_idx, unsigned long rate, @@ -1199,8 +1269,43 @@ static void rp1_clock_choose_div_and_pra struct clk_hw *parent; u32 div; u64 tmp; + int i; parent = clk_hw_get_parent_by_index(hw, parent_idx); + + for (i = 0; i < ARRAY_SIZE(rp1_clk_chg_tree); i++) { + const struct rp1_clk_change *chg = &rp1_clk_chg_tree[i]; + + if (chg->hw == hw && chg->new_rate == rate) { + if (i == 2) + *prate = clk_hw_get_rate(clk_xosc); + else if (parent == rp1_clk_chg_tree[i + 1].hw) + *prate = rp1_clk_chg_tree[i + 1].new_rate; + else + continue; + *calc_rate = chg->new_rate; + return; + } + } + + if (hw == clk_i2s && parent == clk_audio) { + unsigned long core_rate, audio_rate, i2s_rate; + int div_prim, div_clk; + + core_rate = calc_core_pll_rate(parent, rate, &div_prim, &div_clk); + audio_rate = DIV_NEAREST(core_rate, div_prim); + i2s_rate = DIV_NEAREST(audio_rate, div_clk); + rp1_clk_chg_tree[2].hw = clk_hw_get_parent(parent); + rp1_clk_chg_tree[2].new_rate = core_rate; + rp1_clk_chg_tree[1].hw = clk_audio; + rp1_clk_chg_tree[1].new_rate = audio_rate; + rp1_clk_chg_tree[0].hw = clk_i2s; + rp1_clk_chg_tree[0].new_rate = i2s_rate; + *prate = audio_rate; + *calc_rate = i2s_rate; + return; + } + *prate = clk_hw_get_rate(parent); div = rp1_clock_choose_div(rate, *prate, data); @@ -1608,6 +1713,7 @@ static const struct rp1_clk_desc clk_des .source_pll = "pll_audio_core", .ctrl_reg = PLL_AUDIO_PRIM, .fc0_src = FC_NUM(4, 2), + .flags = CLK_SET_RATE_PARENT, ), [RP1_PLL_VIDEO] = REGISTER_PLL( @@ -1850,6 +1956,7 @@ static const struct rp1_clk_desc clk_des .div_int_max = DIV_INT_8BIT_MAX, .max_freq = 50 * MHz, .fc0_src = FC_NUM(4, 4), + .flags = CLK_SET_RATE_PARENT, ), [RP1_CLK_MIPI0_CFG] = REGISTER_CLK( @@ -2272,8 +2379,14 @@ static int rp1_clk_probe(struct platform for (i = 0; i < asize; i++) { desc = &clk_desc_array[i]; - if (desc->clk_register && desc->data) + if (desc->clk_register && desc->data) { hws[i] = desc->clk_register(clockman, desc->data); + if (!strcmp(clk_hw_get_name(hws[i]), "clk_i2s")) { + clk_i2s = hws[i]; + clk_xosc = clk_hw_get_parent_by_index(clk_i2s, 0); + clk_audio = clk_hw_get_parent_by_index(clk_i2s, 1); + } + } } ret = of_clk_add_hw_provider(dev->of_node, of_clk_hw_onecell_get,