diff --git a/drivers/clk/at91/at91rm9200.c b/drivers/clk/at91/at91rm9200.c index 428a6f4b9ebc..fff4fdda974f 100644 --- a/drivers/clk/at91/at91rm9200.c +++ b/drivers/clk/at91/at91rm9200.c @@ -152,7 +152,7 @@ static void __init at91rm9200_pmc_setup(struct device_node *np) "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; diff --git a/drivers/clk/at91/at91sam9260.c b/drivers/clk/at91/at91sam9260.c index b29843bea278..79802f864ee5 100644 --- a/drivers/clk/at91/at91sam9260.c +++ b/drivers/clk/at91/at91sam9260.c @@ -429,7 +429,7 @@ static void __init at91sam926x_pmc_setup(struct device_node *np, &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; diff --git a/drivers/clk/at91/at91sam9g45.c b/drivers/clk/at91/at91sam9g45.c index 15da0dfe3ef2..7ed984f8058c 100644 --- a/drivers/clk/at91/at91sam9g45.c +++ b/drivers/clk/at91/at91sam9g45.c @@ -164,7 +164,7 @@ static void __init at91sam9g45_pmc_setup(struct device_node *np) &at91rm9200_master_layout, &mck_characteristics, &at91sam9g45_mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; diff --git a/drivers/clk/at91/at91sam9n12.c b/drivers/clk/at91/at91sam9n12.c index 7fe435f4b46b..63cc58944b00 100644 --- a/drivers/clk/at91/at91sam9n12.c +++ b/drivers/clk/at91/at91sam9n12.c @@ -191,7 +191,7 @@ static void __init at91sam9n12_pmc_setup(struct device_node *np) &at91sam9x5_master_layout, &mck_characteristics, &at91sam9n12_mck_lock, - CLK_SET_RATE_GATE); + CLK_SET_RATE_GATE, 0); if (IS_ERR(hw)) goto err_free; diff --git a/drivers/clk/at91/at91sam9rl.c b/drivers/clk/at91/at91sam9rl.c index ecbabf5162bd..4d4faf6c61d8 100644 --- a/drivers/clk/at91/at91sam9rl.c +++ b/drivers/clk/at91/at91sam9rl.c @@ -132,7 +132,7 @@ static void __init at91sam9rl_pmc_setup(struct device_node *np) "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; diff --git a/drivers/clk/at91/at91sam9x5.c b/drivers/clk/at91/at91sam9x5.c index 5cce48c64ea2..bd8007b4f3e0 100644 --- a/drivers/clk/at91/at91sam9x5.c +++ b/drivers/clk/at91/at91sam9x5.c @@ -210,7 +210,7 @@ static void __init at91sam9x5_pmc_setup(struct device_node *np, "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; diff --git a/drivers/clk/at91/clk-master.c b/drivers/clk/at91/clk-master.c index e67bcd03a827..b2d0a7f4f7f9 100644 --- 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_ops = { .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_context_chg(struct clk_hw *hw) 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 regmap *regmap, 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 regmap *regmap, 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 regmap *regmap, 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 diff --git a/drivers/clk/at91/dt-compat.c b/drivers/clk/at91/dt-compat.c index a97b99c2dc12..ca2dbb65b9df 100644 --- 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_node *np, 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; diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h index 207ecccef29f..3a1bf6194c28 100644 --- a/drivers/clk/at91/pmc.h +++ b/drivers/clk/at91/pmc.h @@ -182,7 +182,7 @@ at91_clk_register_master_div(struct regmap *regmap, const char *name, 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, diff --git a/drivers/clk/at91/sama5d2.c b/drivers/clk/at91/sama5d2.c index 3d1f78176c3e..d027294a0089 100644 --- a/drivers/clk/at91/sama5d2.c +++ b/drivers/clk/at91/sama5d2.c @@ -249,7 +249,7 @@ static void __init sama5d2_pmc_setup(struct device_node *np) "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; diff --git a/drivers/clk/at91/sama5d3.c b/drivers/clk/at91/sama5d3.c index d376257807d2..339d0f382ff0 100644 --- a/drivers/clk/at91/sama5d3.c +++ b/drivers/clk/at91/sama5d3.c @@ -184,7 +184,7 @@ static void __init sama5d3_pmc_setup(struct device_node *np) "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; diff --git a/drivers/clk/at91/sama5d4.c b/drivers/clk/at91/sama5d4.c index 5cbaac68da44..4af75b1e39e9 100644 --- a/drivers/clk/at91/sama5d4.c +++ b/drivers/clk/at91/sama5d4.c @@ -199,7 +199,7 @@ static void __init sama5d4_pmc_setup(struct device_node *np) "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; diff --git a/drivers/clk/at91/sama7g5.c b/drivers/clk/at91/sama7g5.c index ae52c10af040..c66bde6f7b47 100644 --- a/drivers/clk/at91/sama7g5.c +++ b/drivers/clk/at91/sama7g5.c @@ -1003,7 +1003,7 @@ static void __init sama7g5_pmc_setup(struct device_node *np) hw = at91_clk_register_master_div(regmap, "mck0", "cpuck", &mck0_layout, &mck0_characteristics, - &pmc_mck0_lock, 0); + &pmc_mck0_lock, CLK_GET_RATE_NOCACHE, 5); if (IS_ERR(hw)) goto err_free;