From 75d5d1d584ae73ba0c36d1d7255db6153ca4d3f3 Mon Sep 17 00:00:00 2001 From: Claudiu Beznea Date: Mon, 11 Oct 2021 14:27:16 +0300 Subject: [PATCH 244/247] clk: at91: clk-master: add notifier for divider SAMA7G5 supports DVFS by changing cpuck. On SAMA7G5 mck0 shares the same parent with cpuck as seen in the following clock tree: +----------> cpuck | FRAC PLL ---> DIV PLL -+-> DIV ---> mck0 mck0 could go b/w 32KHz and 200MHz on SAMA7G5. To avoid mck0 overclocking while changing FRAC PLL or DIV PLL the commit implements a notifier for mck0 which applies a safe divider to register (maximum value of the divider which is 5) on PRE_RATE_CHANGE events (such that changes on PLL to not overclock mck0) and sets the maximum allowed rate on POST_RATE_CHANGE events. Signed-off-by: Claudiu Beznea Link: https://lore.kernel.org/r/20211011112719.3951784-13-claudiu.beznea@microchip.com Acked-by: Nicolas Ferre Signed-off-by: Stephen Boyd --- drivers/clk/at91/at91rm9200.c | 2 +- drivers/clk/at91/at91sam9260.c | 2 +- drivers/clk/at91/at91sam9g45.c | 2 +- drivers/clk/at91/at91sam9n12.c | 2 +- drivers/clk/at91/at91sam9rl.c | 2 +- drivers/clk/at91/at91sam9x5.c | 2 +- drivers/clk/at91/clk-master.c | 244 +++++++++++++++++++++++---------- drivers/clk/at91/dt-compat.c | 2 +- drivers/clk/at91/pmc.h | 2 +- drivers/clk/at91/sama5d2.c | 2 +- drivers/clk/at91/sama5d3.c | 2 +- drivers/clk/at91/sama5d4.c | 2 +- drivers/clk/at91/sama7g5.c | 2 +- 13 files changed, 186 insertions(+), 82 deletions(-) --- a/drivers/clk/at91/at91rm9200.c +++ b/drivers/clk/at91/at91rm9200.c @@ -152,7 +152,7 @@ static void __init at91rm9200_pmc_setup( "masterck_pres", &at91rm9200_master_layout, &rm9200_mck_characteristics, - &rm9200_mck_lock, CLK_SET_RATE_GATE); + &rm9200_mck_lock, CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/at91sam9260.c +++ b/drivers/clk/at91/at91sam9260.c @@ -429,7 +429,7 @@ static void __init at91sam926x_pmc_setup &at91rm9200_master_layout, data->mck_characteristics, &at91sam9260_mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/at91sam9g45.c +++ b/drivers/clk/at91/at91sam9g45.c @@ -164,7 +164,7 @@ static void __init at91sam9g45_pmc_setup &at91rm9200_master_layout, &mck_characteristics, &at91sam9g45_mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/at91sam9n12.c +++ b/drivers/clk/at91/at91sam9n12.c @@ -191,7 +191,7 @@ static void __init at91sam9n12_pmc_setup &at91sam9x5_master_layout, &mck_characteristics, &at91sam9n12_mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/at91sam9rl.c +++ b/drivers/clk/at91/at91sam9rl.c @@ -132,7 +132,7 @@ static void __init at91sam9rl_pmc_setup( "masterck_pres", &at91rm9200_master_layout, &sam9rl_mck_characteristics, - &sam9rl_mck_lock, CLK_SET_RATE_GATE); + &sam9rl_mck_lock, CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/at91sam9x5.c +++ b/drivers/clk/at91/at91sam9x5.c @@ -210,7 +210,7 @@ static void __init at91sam9x5_pmc_setup( "masterck_pres", &at91sam9x5_master_layout, &mck_characteristics, &mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/clk-master.c +++ b/drivers/clk/at91/clk-master.c @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -36,8 +37,12 @@ struct clk_master { u8 id; u8 parent; u8 div; + u32 safe_div; }; +/* MCK div reference to be used by notifier. */ +static struct clk_master *master_div; + static inline bool clk_master_ready(struct clk_master *master) { unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY; @@ -153,107 +158,81 @@ static const struct clk_ops master_div_o .restore_context = clk_master_div_restore_context, }; -static int clk_master_div_set_rate(struct clk_hw *hw, unsigned long rate, - unsigned long parent_rate) +/* This function must be called with lock acquired. */ +static int clk_master_div_set(struct clk_master *master, + unsigned long parent_rate, int div) { - struct clk_master *master = to_clk_master(hw); const struct clk_master_characteristics *characteristics = master->characteristics; - unsigned long flags; - unsigned int mckr, tmp; - int div, i; + unsigned long rate = parent_rate; + unsigned int max_div = 0, div_index = 0, max_div_index = 0; + unsigned int i, mckr, tmp; int ret; - div = DIV_ROUND_CLOSEST(parent_rate, rate); - if (div > ARRAY_SIZE(characteristics->divisors)) - return -EINVAL; - for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { if (!characteristics->divisors[i]) break; - if (div == characteristics->divisors[i]) { - div = i; - break; + if (div == characteristics->divisors[i]) + div_index = i; + + if (max_div < characteristics->divisors[i]) { + max_div = characteristics->divisors[i]; + max_div_index = i; } } - if (i == ARRAY_SIZE(characteristics->divisors)) - return -EINVAL; + if (div > max_div) + div_index = max_div_index; - spin_lock_irqsave(master->lock, flags); ret = regmap_read(master->regmap, master->layout->offset, &mckr); if (ret) - goto unlock; + return ret; mckr &= master->layout->mask; tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; - if (tmp == div) - goto unlock; + if (tmp == div_index) + return 0; + + rate /= characteristics->divisors[div_index]; + if (rate < characteristics->output.min) + pr_warn("master clk div is underclocked"); + else if (rate > characteristics->output.max) + pr_warn("master clk div is overclocked"); mckr &= ~(MASTER_DIV_MASK << MASTER_DIV_SHIFT); - mckr |= (div << MASTER_DIV_SHIFT); + mckr |= (div_index << MASTER_DIV_SHIFT); ret = regmap_write(master->regmap, master->layout->offset, mckr); if (ret) - goto unlock; + return ret; while (!clk_master_ready(master)) cpu_relax(); -unlock: - spin_unlock_irqrestore(master->lock, flags); + + master->div = characteristics->divisors[div_index]; return 0; } -static int clk_master_div_determine_rate(struct clk_hw *hw, - struct clk_rate_request *req) +static unsigned long clk_master_div_recalc_rate_chg(struct clk_hw *hw, + unsigned long parent_rate) { struct clk_master *master = to_clk_master(hw); - const struct clk_master_characteristics *characteristics = - master->characteristics; - struct clk_hw *parent; - unsigned long parent_rate, tmp_rate, best_rate = 0; - int i, best_diff = INT_MIN, tmp_diff; - - parent = clk_hw_get_parent(hw); - if (!parent) - return -EINVAL; - - parent_rate = clk_hw_get_rate(parent); - if (!parent_rate) - return -EINVAL; - for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { - if (!characteristics->divisors[i]) - break; - - tmp_rate = DIV_ROUND_CLOSEST_ULL(parent_rate, - characteristics->divisors[i]); - tmp_diff = abs(tmp_rate - req->rate); - - if (!best_rate || best_diff > tmp_diff) { - best_diff = tmp_diff; - best_rate = tmp_rate; - } - - if (!best_diff) - break; - } - - req->best_parent_rate = best_rate; - req->best_parent_hw = parent; - req->rate = best_rate; - - return 0; + return DIV_ROUND_CLOSEST_ULL(parent_rate, master->div); } static void clk_master_div_restore_context_chg(struct clk_hw *hw) { struct clk_master *master = to_clk_master(hw); + unsigned long flags; int ret; - ret = clk_master_div_set_rate(hw, master->pms.rate, - master->pms.parent_rate); + spin_lock_irqsave(master->lock, flags); + ret = clk_master_div_set(master, master->pms.parent_rate, + DIV_ROUND_CLOSEST(master->pms.parent_rate, + master->pms.rate)); + spin_unlock_irqrestore(master->lock, flags); if (ret) pr_warn("Failed to restore MCK DIV clock\n"); } @@ -261,13 +240,116 @@ static void clk_master_div_restore_conte static const struct clk_ops master_div_ops_chg = { .prepare = clk_master_prepare, .is_prepared = clk_master_is_prepared, - .recalc_rate = clk_master_div_recalc_rate, - .determine_rate = clk_master_div_determine_rate, - .set_rate = clk_master_div_set_rate, + .recalc_rate = clk_master_div_recalc_rate_chg, .save_context = clk_master_div_save_context, .restore_context = clk_master_div_restore_context_chg, }; +static int clk_master_div_notifier_fn(struct notifier_block *notifier, + unsigned long code, void *data) +{ + const struct clk_master_characteristics *characteristics = + master_div->characteristics; + struct clk_notifier_data *cnd = data; + unsigned long flags, new_parent_rate, new_rate; + unsigned int mckr, div, new_div = 0; + int ret, i; + long tmp_diff; + long best_diff = -1; + + spin_lock_irqsave(master_div->lock, flags); + switch (code) { + case PRE_RATE_CHANGE: + /* + * We want to avoid any overclocking of MCK DIV domain. To do + * this we set a safe divider (the underclocking is not of + * interest as we can go as low as 32KHz). The relation + * b/w this clock and its parents are as follows: + * + * FRAC PLL -> DIV PLL -> MCK DIV + * + * With the proper safe divider we should be good even with FRAC + * PLL at its maximum value. + */ + ret = regmap_read(master_div->regmap, master_div->layout->offset, + &mckr); + if (ret) { + ret = NOTIFY_STOP_MASK; + goto unlock; + } + + mckr &= master_div->layout->mask; + div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + + /* Switch to safe divider. */ + clk_master_div_set(master_div, + cnd->old_rate * characteristics->divisors[div], + master_div->safe_div); + break; + + case POST_RATE_CHANGE: + /* + * At this point we want to restore MCK DIV domain to its maximum + * allowed rate. + */ + ret = regmap_read(master_div->regmap, master_div->layout->offset, + &mckr); + if (ret) { + ret = NOTIFY_STOP_MASK; + goto unlock; + } + + mckr &= master_div->layout->mask; + div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + new_parent_rate = cnd->new_rate * characteristics->divisors[div]; + + for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { + if (!characteristics->divisors[i]) + break; + + new_rate = DIV_ROUND_CLOSEST_ULL(new_parent_rate, + characteristics->divisors[i]); + + tmp_diff = characteristics->output.max - new_rate; + if (tmp_diff < 0) + continue; + + if (best_diff < 0 || best_diff > tmp_diff) { + new_div = characteristics->divisors[i]; + best_diff = tmp_diff; + } + + if (!tmp_diff) + break; + } + + if (!new_div) { + ret = NOTIFY_STOP_MASK; + goto unlock; + } + + /* Update the div to preserve MCK DIV clock rate. */ + clk_master_div_set(master_div, new_parent_rate, + new_div); + + ret = NOTIFY_OK; + break; + + default: + ret = NOTIFY_DONE; + break; + } + +unlock: + spin_unlock_irqrestore(master_div->lock, flags); + + return ret; +} + +static struct notifier_block clk_master_div_notifier = { + .notifier_call = clk_master_div_notifier_fn, +}; + static void clk_sama7g5_master_best_diff(struct clk_rate_request *req, struct clk_hw *parent, unsigned long parent_rate, @@ -496,6 +578,8 @@ at91_clk_register_master_internal(struct struct clk_master *master; struct clk_init_data init; struct clk_hw *hw; + unsigned int mckr; + unsigned long irqflags; int ret; if (!name || !num_parents || !parent_names || !lock) @@ -518,6 +602,16 @@ at91_clk_register_master_internal(struct master->chg_pid = chg_pid; master->lock = lock; + if (ops == &master_div_ops_chg) { + spin_lock_irqsave(master->lock, irqflags); + regmap_read(master->regmap, master->layout->offset, &mckr); + spin_unlock_irqrestore(master->lock, irqflags); + + mckr &= layout->mask; + mckr = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + master->div = characteristics->divisors[mckr]; + } + hw = &master->hw; ret = clk_hw_register(NULL, &master->hw); if (ret) { @@ -554,19 +648,29 @@ at91_clk_register_master_div(struct regm const char *name, const char *parent_name, const struct clk_master_layout *layout, const struct clk_master_characteristics *characteristics, - spinlock_t *lock, u32 flags) + spinlock_t *lock, u32 flags, u32 safe_div) { const struct clk_ops *ops; + struct clk_hw *hw; if (flags & CLK_SET_RATE_GATE) ops = &master_div_ops; else ops = &master_div_ops_chg; - return at91_clk_register_master_internal(regmap, name, 1, - &parent_name, layout, - characteristics, ops, - lock, flags, -EINVAL); + hw = at91_clk_register_master_internal(regmap, name, 1, + &parent_name, layout, + characteristics, ops, + lock, flags, -EINVAL); + + if (!IS_ERR(hw) && safe_div) { + master_div = to_clk_master(hw); + master_div->safe_div = safe_div; + clk_notifier_register(hw->clk, + &clk_master_div_notifier); + } + + return hw; } static unsigned long --- a/drivers/clk/at91/dt-compat.c +++ b/drivers/clk/at91/dt-compat.c @@ -399,7 +399,7 @@ of_at91_clk_master_setup(struct device_n hw = at91_clk_register_master_div(regmap, name, "masterck_pres", layout, characteristics, - &mck_lock, CLK_SET_RATE_GATE); + &mck_lock, CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto out_free_characteristics; --- a/drivers/clk/at91/pmc.h +++ b/drivers/clk/at91/pmc.h @@ -182,7 +182,7 @@ at91_clk_register_master_div(struct regm const char *parent_names, const struct clk_master_layout *layout, const struct clk_master_characteristics *characteristics, - spinlock_t *lock, u32 flags); + spinlock_t *lock, u32 flags, u32 safe_div); struct clk_hw * __init at91_clk_sama7g5_register_master(struct regmap *regmap, --- a/drivers/clk/at91/sama5d2.c +++ b/drivers/clk/at91/sama5d2.c @@ -249,7 +249,7 @@ static void __init sama5d2_pmc_setup(str "masterck_pres", &at91sam9x5_master_layout, &mck_characteristics, &mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/sama5d3.c +++ b/drivers/clk/at91/sama5d3.c @@ -184,7 +184,7 @@ static void __init sama5d3_pmc_setup(str "masterck_pres", &at91sam9x5_master_layout, &mck_characteristics, &mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/sama5d4.c +++ b/drivers/clk/at91/sama5d4.c @@ -199,7 +199,7 @@ static void __init sama5d4_pmc_setup(str "masterck_pres", &at91sam9x5_master_layout, &mck_characteristics, &mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; --- a/drivers/clk/at91/sama7g5.c +++ b/drivers/clk/at91/sama7g5.c @@ -993,7 +993,7 @@ static void __init sama7g5_pmc_setup(str parent_names[0] = "cpupll_divpmcck"; hw = at91_clk_register_master_div(regmap, "mck0", "cpupll_divpmcck", &mck0_layout, &mck0_characteristics, - &pmc_mck0_lock, 0); + &pmc_mck0_lock, CLK_GET_RATE_NOCACHE, 5); if (IS_ERR(hw)) goto err_free;