Re: [PATCH v5 12/15] clk: at91: clk-master: add notifier for divider

[Nicolas Ferre <nicolas.ferre@microchip.com>]

On 11/10/2021 at 13:27, Claudiu Beznea wrote:
> 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 <claudiu.beznea@microchip.com>

looks good to me:
Acked-by: Nicolas Ferre <nicolas.ferre@microchip.com>

> ---
>   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(-)
> 
> 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 <linux/clk-provider.h>
>   #include <linux/clkdev.h>
> +#include <linux/clk.h>
>   #include <linux/clk/at91_pmc.h>
>   #include <linux/of.h>
>   #include <linux/mfd/syscon.h>
> @@ -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;
>   
> 


-- 
Nicolas Ferre

_______________________________________________
linux-arm-kernel mailing list
linux-arm-kernel@lists.infradead.org
http://lists.infradead.org/mailman/listinfo/linux-arm-kernel