diff options
Diffstat (limited to '')
| -rw-r--r-- | drivers/clk/mmp/clk-frac.c | 206 |
1 files changed, 206 insertions, 0 deletions
diff --git a/drivers/clk/mmp/clk-frac.c b/drivers/clk/mmp/clk-frac.c new file mode 100644 index 000000000..1b90867b6 --- /dev/null +++ b/drivers/clk/mmp/clk-frac.c @@ -0,0 +1,206 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * mmp factor clock operation source file + * + * Copyright (C) 2012 Marvell + * Chao Xie <xiechao.mail@gmail.com> + */ + +#include <linux/clk-provider.h> +#include <linux/slab.h> +#include <linux/io.h> +#include <linux/err.h> + +#include "clk.h" +/* + * It is M/N clock + * + * Fout from synthesizer can be given from two equations: + * numerator/denominator = Fin / (Fout * factor) + */ + +#define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw) + +static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate, + unsigned long *prate) +{ + struct mmp_clk_factor *factor = to_clk_factor(hw); + u64 rate = 0, prev_rate; + int i; + + for (i = 0; i < factor->ftbl_cnt; i++) { + prev_rate = rate; + rate = *prate; + rate *= factor->ftbl[i].den; + do_div(rate, factor->ftbl[i].num * factor->masks->factor); + + if (rate > drate) + break; + } + if ((i == 0) || (i == factor->ftbl_cnt)) { + return rate; + } else { + if ((drate - prev_rate) > (rate - drate)) + return rate; + else + return prev_rate; + } +} + +static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct mmp_clk_factor *factor = to_clk_factor(hw); + struct mmp_clk_factor_masks *masks = factor->masks; + unsigned int val, num, den; + u64 rate; + + val = readl_relaxed(factor->base); + + /* calculate numerator */ + num = (val >> masks->num_shift) & masks->num_mask; + + /* calculate denominator */ + den = (val >> masks->den_shift) & masks->den_mask; + + if (!den) + return 0; + + rate = parent_rate; + rate *= den; + do_div(rate, num * factor->masks->factor); + + return rate; +} + +/* Configures new clock rate*/ +static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, + unsigned long prate) +{ + struct mmp_clk_factor *factor = to_clk_factor(hw); + struct mmp_clk_factor_masks *masks = factor->masks; + int i; + unsigned long val; + unsigned long flags = 0; + u64 rate = 0; + + for (i = 0; i < factor->ftbl_cnt; i++) { + rate = prate; + rate *= factor->ftbl[i].den; + do_div(rate, factor->ftbl[i].num * factor->masks->factor); + + if (rate > drate) + break; + } + if (i > 0) + i--; + + if (factor->lock) + spin_lock_irqsave(factor->lock, flags); + + val = readl_relaxed(factor->base); + + val &= ~(masks->num_mask << masks->num_shift); + val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; + + val &= ~(masks->den_mask << masks->den_shift); + val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; + + writel_relaxed(val, factor->base); + + if (factor->lock) + spin_unlock_irqrestore(factor->lock, flags); + + return 0; +} + +static int clk_factor_init(struct clk_hw *hw) +{ + struct mmp_clk_factor *factor = to_clk_factor(hw); + struct mmp_clk_factor_masks *masks = factor->masks; + u32 val, num, den; + int i; + unsigned long flags = 0; + + if (factor->lock) + spin_lock_irqsave(factor->lock, flags); + + val = readl(factor->base); + + /* calculate numerator */ + num = (val >> masks->num_shift) & masks->num_mask; + + /* calculate denominator */ + den = (val >> masks->den_shift) & masks->den_mask; + + for (i = 0; i < factor->ftbl_cnt; i++) + if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) + break; + + if (i >= factor->ftbl_cnt) { + val &= ~(masks->num_mask << masks->num_shift); + val |= (factor->ftbl[0].num & masks->num_mask) << + masks->num_shift; + + val &= ~(masks->den_mask << masks->den_shift); + val |= (factor->ftbl[0].den & masks->den_mask) << + masks->den_shift; + } + + if (!(val & masks->enable_mask) || i >= factor->ftbl_cnt) { + val |= masks->enable_mask; + writel(val, factor->base); + } + + if (factor->lock) + spin_unlock_irqrestore(factor->lock, flags); + + return 0; +} + +static const struct clk_ops clk_factor_ops = { + .recalc_rate = clk_factor_recalc_rate, + .round_rate = clk_factor_round_rate, + .set_rate = clk_factor_set_rate, + .init = clk_factor_init, +}; + +struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, + unsigned long flags, void __iomem *base, + struct mmp_clk_factor_masks *masks, + struct mmp_clk_factor_tbl *ftbl, + unsigned int ftbl_cnt, spinlock_t *lock) +{ + struct mmp_clk_factor *factor; + struct clk_init_data init; + struct clk *clk; + + if (!masks) { + pr_err("%s: must pass a clk_factor_mask\n", __func__); + return ERR_PTR(-EINVAL); + } + + factor = kzalloc(sizeof(*factor), GFP_KERNEL); + if (!factor) + return ERR_PTR(-ENOMEM); + + /* struct clk_aux assignments */ + factor->base = base; + factor->masks = masks; + factor->ftbl = ftbl; + factor->ftbl_cnt = ftbl_cnt; + factor->hw.init = &init; + factor->lock = lock; + + init.name = name; + init.ops = &clk_factor_ops; + init.flags = flags; + init.parent_names = &parent_name; + init.num_parents = 1; + + clk = clk_register(NULL, &factor->hw); + if (IS_ERR_OR_NULL(clk)) + kfree(factor); + + return clk; +} |