[U-Boot] [PATCH u-boot-marvell v2 02/12] clk: armada-37xx-periph: Support changing clock parent and rate

[Stefan Roese]

On 17.08.2018 12:58, Marek Behún wrote:
> Add support for changing clock rate and parent clock for Armada 37xx
> peripheral clocks.
> 
> Only clocks which can be disabled (.can_gate is true) can have parent
> or rate changed.
> 
> This is needed so that Turris Mox can change SPI clock in device tree.
> 
> Signed-off-by: Marek Behun <marek.behun at nic.cz>
> ---
>   drivers/clk/mvebu/armada-37xx-periph.c | 130 ++++++++++++++++++++++++++++++++-
>   1 file changed, 129 insertions(+), 1 deletion(-)
> 
> diff --git a/drivers/clk/mvebu/armada-37xx-periph.c b/drivers/clk/mvebu/armada-37xx-periph.c
> index 902a6cc9ef..b1a35968e1 100644
> --- a/drivers/clk/mvebu/armada-37xx-periph.c
> +++ b/drivers/clk/mvebu/armada-37xx-periph.c
> @@ -224,11 +224,21 @@ static const struct clk_periph clks_sb[] = {
>   	{ },
>   };
>   
> -static inline int get_mux(struct a37xx_periphclk *priv, int shift)
> +static int get_mux(struct a37xx_periphclk *priv, int shift)
>   {
>   	return (readl(priv->reg + TBG_SEL) >> shift) & 3;
>   }
>   
> +static void set_mux(struct a37xx_periphclk *priv, int shift, int val)
> +{
> +	u32 reg;
> +
> +	reg = readl(priv->reg + TBG_SEL);
> +	reg &= ~(3 << shift);
> +	reg |= (val & 3) << shift;
> +	writel(reg, priv->reg + TBG_SEL);
> +}
> +
>   static ulong periph_clk_get_rate(struct a37xx_periphclk *priv, int id);
>   
>   static ulong get_parent_rate(struct a37xx_periphclk *priv, int id)
> @@ -277,6 +287,17 @@ static ulong get_div(struct a37xx_periphclk *priv,
>   	return 0;
>   }
>   
> +static void set_div_val(struct a37xx_periphclk *priv,
> +			const struct clk_periph *clk, int idx, int val)
> +{
> +	u32 reg;
> +
> +	reg = readl(priv->reg + clk->div_reg_off[idx]);
> +	reg &= ~(clk->div_mask[idx] << clk->div_shift[idx]);
> +	reg |= (val & clk->div_mask[idx]) << clk->div_shift[idx];
> +	writel(reg, priv->reg + clk->div_reg_off[idx]);
> +}
> +
>   static ulong periph_clk_get_rate(struct a37xx_periphclk *priv, int id)
>   {
>   	const struct clk_periph *clk = &priv->clks[id];
> @@ -337,6 +358,111 @@ static int armada_37xx_periph_clk_disable(struct clk *clk)
>   	return periph_clk_enable(clk, 0);
>   }
>   
> +#define diff(a, b) abs((long)(a) - (long)(b))
> +
> +static ulong find_best_div(const struct clk_div_table *t0,
> +			   const struct clk_div_table *t1, ulong parent_rate,
> +			   ulong req_rate, int *v0, int *v1)
> +{
> +	const struct clk_div_table *i, *j;
> +	ulong rate, best_rate = 0;
> +
> +	for (i = t0; i && i->div; ++i) {
> +		for (j = t1; j && j->div; ++j) {
> +			rate = DIV_ROUND_UP(parent_rate, i->div * j->div);
> +
> +			if (!best_rate ||
> +			    diff(rate, req_rate) < diff(best_rate, req_rate)) {
> +				best_rate = rate;
> +				*v0 = i->val;
> +				*v1 = j->val;
> +			}
> +		}
> +	}
> +
> +	return best_rate;
> +}
> +
> +static ulong armada_37xx_periph_clk_set_rate(struct clk *clk, ulong req_rate)
> +{
> +	struct a37xx_periphclk *priv = dev_get_priv(clk->dev);
> +	const struct clk_periph *periph_clk = &priv->clks[clk->id];
> +	ulong rate, old_rate, parent_rate;
> +	int div_val0 = 0, div_val1 = 0;
> +	const struct clk_div_table *t1;
> +	static const struct clk_div_table empty_table[2] = {
> +		{ 1, 0 },
> +		{ 0, 0 }
> +	};
> +
> +	if (clk->id > priv->count)
> +		return -EINVAL;
> +
> +	old_rate = periph_clk_get_rate(priv, clk->id);
> +	if (old_rate == -EINVAL)
> +		return -EINVAL;
> +
> +	if (old_rate == req_rate)
> +		return old_rate;
> +
> +	if (!periph_clk->can_gate || !periph_clk->dividers)
> +		return -ENOTSUPP;
> +
> +	parent_rate = get_parent_rate(priv, clk->id);
> +	if (parent_rate == -EINVAL)
> +		return -EINVAL;
> +
> +	t1 = empty_table;
> +	if (periph_clk->dividers > 1)
> +		t1 = periph_clk->div_table[1];
> +
> +	rate = find_best_div(periph_clk->div_table[0], t1, parent_rate,
> +			     req_rate, &div_val0, &div_val1);
> +
> +	periph_clk_enable(clk, 0);
> +
> +	set_div_val(priv, periph_clk, 0, div_val0);
> +	if (periph_clk->dividers > 1)
> +		set_div_val(priv, periph_clk, 1, div_val1);
> +
> +	periph_clk_enable(clk, 1);
> +
> +	return rate;
> +}
> +
> +static int armada_37xx_periph_clk_set_parent(struct clk *clk,
> +					     struct clk *parent)
> +{
> +	struct a37xx_periphclk *priv = dev_get_priv(clk->dev);
> +	const struct clk_periph *periph_clk = &priv->clks[clk->id];
> +	struct clk check_parent;
> +	int ret;
> +
> +	/* We also check if parent is our TBG clock */
> +	if (clk->id > priv->count || parent->id >= MAX_TBG_PARENTS)
> +		return -EINVAL;
> +
> +	if (!periph_clk->can_mux || !periph_clk->can_gate)
> +		return -ENOTSUPP;
> +
> +	ret = clk_get_by_index(clk->dev, 0, &check_parent);
> +	if (ret < 0)
> +		return ret;
> +
> +	if (parent->dev != check_parent.dev)
> +		ret = -EINVAL;
> +
> +	clk_free(&check_parent);
> +	if (ret < 0)
> +		return ret;
> +
> +	periph_clk_enable(clk, 0);
> +	set_mux(priv, periph_clk->mux_shift, parent->id);
> +	periph_clk_enable(clk, 1);
> +
> +	return 0;
> +}
> +
>   #if defined(CONFIG_CMD_CLK) && defined(CONFIG_CLK_ARMADA_3720)
>   static int armada_37xx_periph_clk_dump(struct udevice *dev)
>   {
> @@ -473,6 +599,8 @@ static int armada_37xx_periph_clk_probe(struct udevice *dev)
>   
>   static const struct clk_ops armada_37xx_periph_clk_ops = {
>   	.get_rate = armada_37xx_periph_clk_get_rate,
> +	.set_rate = armada_37xx_periph_clk_set_rate,
> +	.set_parent = armada_37xx_periph_clk_set_parent,
>   	.enable = armada_37xx_periph_clk_enable,
>   	.disable = armada_37xx_periph_clk_disable,
>   };
> 

Applied to u-boot-marvell/master

Thanks,
Stefan