diff options
Diffstat (limited to 'drivers/clk/xilinx/clk-xlnx-clock-wizard.c')
| -rw-r--r-- | drivers/clk/xilinx/clk-xlnx-clock-wizard.c | 228 |
1 files changed, 204 insertions, 24 deletions
diff --git a/drivers/clk/xilinx/clk-xlnx-clock-wizard.c b/drivers/clk/xilinx/clk-xlnx-clock-wizard.c index eb1dfe7ecc1b..b7591ae019e7 100644 --- a/drivers/clk/xilinx/clk-xlnx-clock-wizard.c +++ b/drivers/clk/xilinx/clk-xlnx-clock-wizard.c @@ -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; - init.ops = &clk_wzrd_clk_divider_ops; + 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); |