Commit 595c88cd authored by Shubhrajyoti Datta's avatar Shubhrajyoti Datta Committed by Stephen Boyd
Browse files

clocking-wizard: Support higher frequency accuracy 



Change the multipliers and divisors to support a higher
frequency accuracy if there is only one output.
Currently only O is changed now we are changing M, D and O.
For multiple output case the earlier behavior is retained.

Signed-off-by: default avatarShubhrajyoti Datta <shubhrajyoti.datta@amd.com>
Link: https://lore.kernel.org/r/20230327062637.22237-1-shubhrajyoti.datta@amd.com


Signed-off-by: default avatarStephen Boyd <sboyd@kernel.org>
parent 57e3bbd2

drivers/clk/xilinx/clk-xlnx-clock-wizard.c

+204 −24
Original line number Diff line number Diff line
@@ -8,12 +8,14 @@ 
 *

 */



#include <linux/bitfield.h>

#include <linux/platform_device.h>

#include <linux/clk.h>

#include <linux/clk-provider.h>

#include <linux/slab.h>

#include <linux/io.h>

#include <linux/of.h>

#include <linux/math64.h>

#include <linux/module.h>

#include <linux/err.h>

#include <linux/iopoll.h>
@@ -37,6 +39,7 @@ 
#define WZRD_CLKOUT_DIVIDE_MASK		(0xff << WZRD_DIVCLK_DIVIDE_SHIFT)

#define WZRD_CLKOUT_FRAC_SHIFT		8

#define WZRD_CLKOUT_FRAC_MASK		0x3ff

#define WZRD_CLKOUT0_FRAC_MASK		GENMASK(17, 8)



#define WZRD_DR_MAX_INT_DIV_VALUE	255

#define WZRD_DR_STATUS_REG_OFFSET	0x04
@@ -49,6 +52,22 @@ 


#define WZRD_USEC_POLL		10

#define WZRD_TIMEOUT_POLL		1000



/* Divider limits, from UG572 Table 3-4 for Ultrascale+ */

#define DIV_O				0x01

#define DIV_ALL				0x03



#define WZRD_M_MIN			2

#define WZRD_M_MAX			128

#define WZRD_D_MIN			1

#define WZRD_D_MAX			106

#define WZRD_VCO_MIN			800000000

#define WZRD_VCO_MAX			1600000000

#define WZRD_O_MIN			1

#define WZRD_O_MAX			128

#define WZRD_MIN_ERR			20000

#define WZRD_FRAC_POINTS		1000



/* Get the mask from width */

#define div_mask(width)			((1 << (width)) - 1)
@@ -97,6 +116,9 @@ struct clk_wzrd { 
 * @width:	width of the divider bit field

 * @flags:	clk_wzrd divider flags

 * @table:	array of value/divider pairs, last entry should have div = 0

 * @m:	value of the multiplier

 * @d:	value of the common divider

 * @o:	value of the leaf divider

 * @lock:	register lock

 */

struct clk_wzrd_divider {
@@ -107,6 +129,9 @@ struct clk_wzrd_divider { 
	u8 width;

	u8 flags;

	const struct clk_div_table *table;

	u32 m;

	u32 d;

	u32 o;

	spinlock_t *lock;  /* divider lock */

};
@@ -198,12 +223,155 @@ static long clk_wzrd_round_rate(struct clk_hw *hw, unsigned long rate, 
	return *prate / div;

}



static int clk_wzrd_get_divisors(struct clk_hw *hw, unsigned long rate,

				 unsigned long parent_rate)

{

	struct clk_wzrd_divider *divider = to_clk_wzrd_divider(hw);

	unsigned long vco_freq, freq, diff;

	u32 m, d, o;



	for (m = WZRD_M_MIN; m <= WZRD_M_MAX; m++) {

		for (d = WZRD_D_MIN; d <= WZRD_D_MAX; d++) {

			vco_freq = DIV_ROUND_CLOSEST((parent_rate * m), d);

			if (vco_freq >= WZRD_VCO_MIN && vco_freq <= WZRD_VCO_MAX) {

				for (o = WZRD_O_MIN; o <= WZRD_O_MAX; o++) {

					freq = DIV_ROUND_CLOSEST_ULL(vco_freq, o);

					diff = abs(freq - rate);



					if (diff < WZRD_MIN_ERR) {

						divider->m = m;

						divider->d = d;

						divider->o = o;

						return 0;

					}

				}

			}

		}

	}

	return -EBUSY;

}



static int clk_wzrd_dynamic_all_nolock(struct clk_hw *hw, unsigned long rate,

				       unsigned long parent_rate)

{

	struct clk_wzrd_divider *divider = to_clk_wzrd_divider(hw);

	unsigned long vco_freq, rate_div, clockout0_div;

	u32 reg, pre, value, f;

	int err;



	err = clk_wzrd_get_divisors(hw, rate, parent_rate);

	if (err)

		return err;



	vco_freq = DIV_ROUND_CLOSEST(parent_rate * divider->m, divider->d);

	rate_div = DIV_ROUND_CLOSEST_ULL((vco_freq * WZRD_FRAC_POINTS), rate);



	clockout0_div = div_u64(rate_div,  WZRD_FRAC_POINTS);



	pre = DIV_ROUND_CLOSEST_ULL(vco_freq * WZRD_FRAC_POINTS, rate);

	f = (pre - (clockout0_div * WZRD_FRAC_POINTS));

	f &= WZRD_CLKOUT_FRAC_MASK;



	reg = FIELD_PREP(WZRD_CLKOUT_DIVIDE_MASK, clockout0_div) |

	      FIELD_PREP(WZRD_CLKOUT0_FRAC_MASK, f);



	writel(reg, divider->base + WZRD_CLK_CFG_REG(2));

	/* Set divisor and clear phase offset */

	reg = FIELD_PREP(WZRD_CLKFBOUT_MULT_MASK, divider->m) |

	      FIELD_PREP(WZRD_DIVCLK_DIVIDE_MASK, divider->d);

	writel(reg, divider->base + WZRD_CLK_CFG_REG(0));

	writel(divider->o, divider->base + WZRD_CLK_CFG_REG(2));

	writel(0, divider->base + WZRD_CLK_CFG_REG(3));

	/* Check status register */

	err = readl_poll_timeout(divider->base + WZRD_DR_STATUS_REG_OFFSET, value,

				 value & WZRD_DR_LOCK_BIT_MASK,

				 WZRD_USEC_POLL, WZRD_TIMEOUT_POLL);

	if (err)

		return -ETIMEDOUT;



	/* Initiate reconfiguration */

	writel(WZRD_DR_BEGIN_DYNA_RECONF,

	       divider->base + WZRD_DR_INIT_REG_OFFSET);



	/* Check status register */

	return readl_poll_timeout(divider->base + WZRD_DR_STATUS_REG_OFFSET, value,

				 value & WZRD_DR_LOCK_BIT_MASK,

				 WZRD_USEC_POLL, WZRD_TIMEOUT_POLL);

}



static int clk_wzrd_dynamic_all(struct clk_hw *hw, unsigned long rate,

				unsigned long parent_rate)

{

	struct clk_wzrd_divider *divider = to_clk_wzrd_divider(hw);

	unsigned long flags = 0;

	int ret;



	spin_lock_irqsave(divider->lock, flags);



	ret = clk_wzrd_dynamic_all_nolock(hw, rate, parent_rate);



	spin_unlock_irqrestore(divider->lock, flags);



	return ret;

}



static unsigned long clk_wzrd_recalc_rate_all(struct clk_hw *hw,

					      unsigned long parent_rate)

{

	struct clk_wzrd_divider *divider = to_clk_wzrd_divider(hw);

	u32 m, d, o, div, reg, f;



	reg = readl(divider->base + WZRD_CLK_CFG_REG(0));

	d = FIELD_GET(WZRD_DIVCLK_DIVIDE_MASK, reg);

	m = FIELD_GET(WZRD_CLKFBOUT_MULT_MASK, reg);

	reg = readl(divider->base + WZRD_CLK_CFG_REG(2));

	o = FIELD_GET(WZRD_DIVCLK_DIVIDE_MASK, reg);

	f = FIELD_GET(WZRD_CLKOUT0_FRAC_MASK, reg);



	div = DIV_ROUND_CLOSEST(d * (WZRD_FRAC_POINTS * o + f), WZRD_FRAC_POINTS);

	return divider_recalc_rate(hw, parent_rate * m, div, divider->table,

			divider->flags, divider->width);

}



static long clk_wzrd_round_rate_all(struct clk_hw *hw, unsigned long rate,

				    unsigned long *prate)

{

	struct clk_wzrd_divider *divider = to_clk_wzrd_divider(hw);

	unsigned long int_freq;

	u32 m, d, o, div, f;

	int err;



	err = clk_wzrd_get_divisors(hw, rate, *prate);

	if (err)

		return err;



	m = divider->m;

	d = divider->d;

	o = divider->o;



	div = d * o;

	int_freq =  divider_recalc_rate(hw, *prate * m, div, divider->table,

					divider->flags, divider->width);



	if (rate > int_freq) {

		f = DIV_ROUND_CLOSEST_ULL(rate * WZRD_FRAC_POINTS, int_freq);

		rate = DIV_ROUND_CLOSEST(int_freq * f, WZRD_FRAC_POINTS);

	}

	return rate;

}



static const struct clk_ops clk_wzrd_clk_divider_ops = {

	.round_rate = clk_wzrd_round_rate,

	.set_rate = clk_wzrd_dynamic_reconfig,

	.recalc_rate = clk_wzrd_recalc_rate,

};



static const struct clk_ops clk_wzrd_clk_div_all_ops = {

	.round_rate = clk_wzrd_round_rate_all,

	.set_rate = clk_wzrd_dynamic_all,

	.recalc_rate = clk_wzrd_recalc_rate_all,

};



static unsigned long clk_wzrd_recalc_ratef(struct clk_hw *hw,

					   unsigned long parent_rate)

{
@@ -280,7 +448,7 @@ static struct clk *clk_wzrd_register_divf(struct device *dev, 
					  void __iomem *base, u16 offset,

					  u8 shift, u8 width,

					  u8 clk_divider_flags,

					  const struct clk_div_table *table,

					  u32 div_type,

					  spinlock_t *lock)

{

	struct clk_wzrd_divider *div;
@@ -307,7 +475,6 @@ static struct clk *clk_wzrd_register_divf(struct device *dev, 
	div->flags = clk_divider_flags;

	div->lock = lock;

	div->hw.init = &init;

	div->table = table;



	hw = &div->hw;

	ret =  devm_clk_hw_register(dev, hw);
@@ -324,7 +491,7 @@ static struct clk *clk_wzrd_register_divider(struct device *dev, 
					     void __iomem *base, u16 offset,

					     u8 shift, u8 width,

					     u8 clk_divider_flags,

					     const struct clk_div_table *table,

					     u32 div_type,

					     spinlock_t *lock)

{

	struct clk_wzrd_divider *div;
@@ -337,7 +504,12 @@ static struct clk *clk_wzrd_register_divider(struct device *dev, 
		return ERR_PTR(-ENOMEM);



	init.name = name;

	if (clk_divider_flags & CLK_DIVIDER_READ_ONLY)

		init.ops = &clk_divider_ro_ops;

	else if (div_type == DIV_O)

		init.ops = &clk_wzrd_clk_divider_ops;

	else

		init.ops = &clk_wzrd_clk_div_all_ops;

	init.flags = flags;

	init.parent_names =  &parent_name;

	init.num_parents =  1;
@@ -349,7 +521,6 @@ static struct clk *clk_wzrd_register_divider(struct device *dev, 
	div->flags = clk_divider_flags;

	div->lock = lock;

	div->hw.init = &init;

	div->table = table;



	hw = &div->hw;

	ret = devm_clk_hw_register(dev, hw);
@@ -425,6 +596,7 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
	const char *clk_name;

	void __iomem *ctrl_reg;

	struct clk_wzrd *clk_wzrd;

	const char *clkout_name;

	struct device_node *np = pdev->dev.of_node;

	int nr_outputs;

	unsigned long flags = 0;
@@ -469,6 +641,26 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
		goto err_disable_clk;

	}



	ret = of_property_read_u32(np, "xlnx,nr-outputs", &nr_outputs);

	if (ret || nr_outputs > WZRD_NUM_OUTPUTS) {

		ret = -EINVAL;

		goto err_disable_clk;

	}



	clkout_name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "%s_out0", dev_name(&pdev->dev));

	if (nr_outputs == 1) {

		clk_wzrd->clkout[0] = clk_wzrd_register_divider

				(&pdev->dev, clkout_name,

				__clk_get_name(clk_wzrd->clk_in1), 0,

				clk_wzrd->base, WZRD_CLK_CFG_REG(3),

				WZRD_CLKOUT_DIVIDE_SHIFT,

				WZRD_CLKOUT_DIVIDE_WIDTH,

				CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO,

				DIV_ALL, &clkwzrd_lock);



		goto out;

	}



	reg = readl(clk_wzrd->base + WZRD_CLK_CFG_REG(0));

	reg_f = reg & WZRD_CLKFBOUT_FRAC_MASK;

	reg_f =  reg_f >> WZRD_CLKFBOUT_FRAC_SHIFT;
@@ -476,20 +668,11 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
	reg = reg & WZRD_CLKFBOUT_MULT_MASK;

	reg =  reg >> WZRD_CLKFBOUT_MULT_SHIFT;

	mult = (reg * 1000) + reg_f;

	clk_name = kasprintf(GFP_KERNEL, "%s_mul", dev_name(&pdev->dev));

	clk_name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "%s_mul", dev_name(&pdev->dev));

	if (!clk_name) {

		ret = -ENOMEM;

		goto err_disable_clk;

	}



	ret = of_property_read_u32(np, "xlnx,nr-outputs", &nr_outputs);

	if (ret || nr_outputs > WZRD_NUM_OUTPUTS) {

		ret = -EINVAL;

		goto err_disable_clk;

	}

	if (nr_outputs == 1)

		flags = CLK_SET_RATE_PARENT;



	clk_wzrd->clks_internal[wzrd_clk_mul] = clk_register_fixed_factor

			(&pdev->dev, clk_name,

			 __clk_get_name(clk_wzrd->clk_in1),
@@ -500,7 +683,7 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
		goto err_disable_clk;

	}



	clk_name = kasprintf(GFP_KERNEL, "%s_mul_div", dev_name(&pdev->dev));

	clk_name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "%s_mul_div", dev_name(&pdev->dev));

	if (!clk_name) {

		ret = -ENOMEM;

		goto err_rm_int_clk;
@@ -521,9 +704,8 @@ static int clk_wzrd_probe(struct platform_device *pdev) 


	/* register div per output */

	for (i = nr_outputs - 1; i >= 0 ; i--) {

		const char *clkout_name;



		clkout_name = kasprintf(GFP_KERNEL, "%s_out%d", dev_name(&pdev->dev), i);

		clkout_name = devm_kasprintf(&pdev->dev, GFP_KERNEL,

					     "%s_out%d", dev_name(&pdev->dev), i);

		if (!clkout_name) {

			ret = -ENOMEM;

			goto err_rm_int_clk;
@@ -537,7 +719,7 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
				WZRD_CLKOUT_DIVIDE_SHIFT,

				WZRD_CLKOUT_DIVIDE_WIDTH,

				CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO,

				NULL, &clkwzrd_lock);

				DIV_O, &clkwzrd_lock);

		else

			clk_wzrd->clkout[i] = clk_wzrd_register_divider

				(&pdev->dev, clkout_name,
@@ -546,7 +728,7 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
				WZRD_CLKOUT_DIVIDE_SHIFT,

				WZRD_CLKOUT_DIVIDE_WIDTH,

				CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO,

				NULL, &clkwzrd_lock);

				DIV_O, &clkwzrd_lock);

		if (IS_ERR(clk_wzrd->clkout[i])) {

			int j;
@@ -559,8 +741,7 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
		}

	}



	kfree(clk_name);



out:

	clk_wzrd->clk_data.clks = clk_wzrd->clkout;

	clk_wzrd->clk_data.clk_num = ARRAY_SIZE(clk_wzrd->clkout);

	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_wzrd->clk_data);
@@ -585,7 +766,6 @@ static int clk_wzrd_probe(struct platform_device *pdev) 
err_rm_int_clks:

	clk_unregister(clk_wzrd->clks_internal[1]);

err_rm_int_clk:

	kfree(clk_name);

	clk_unregister(clk_wzrd->clks_internal[0]);

err_disable_clk:

	clk_disable_unprepare(clk_wzrd->axi_clk);