diff options
Diffstat (limited to 'drivers/clk/at91/clk-master.c')
| -rw-r--r-- | drivers/clk/at91/clk-master.c | 872 |
1 files changed, 872 insertions, 0 deletions
diff --git a/drivers/clk/at91/clk-master.c b/drivers/clk/at91/clk-master.c new file mode 100644 index 000000000..b7cd1924d --- /dev/null +++ b/drivers/clk/at91/clk-master.c @@ -0,0 +1,872 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> + */ + +#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> +#include <linux/regmap.h> + +#include "pmc.h" + +#define MASTER_PRES_MASK 0x7 +#define MASTER_PRES_MAX MASTER_PRES_MASK +#define MASTER_DIV_SHIFT 8 +#define MASTER_DIV_MASK 0x7 + +#define PMC_MCR_CSS_SHIFT (16) + +#define MASTER_MAX_ID 4 + +#define to_clk_master(hw) container_of(hw, struct clk_master, hw) + +struct clk_master { + struct clk_hw hw; + struct regmap *regmap; + spinlock_t *lock; + const struct clk_master_layout *layout; + const struct clk_master_characteristics *characteristics; + struct at91_clk_pms pms; + u32 *mux_table; + u32 mckr; + int chg_pid; + 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; + unsigned int status; + + regmap_read(master->regmap, AT91_PMC_SR, &status); + + return !!(status & bit); +} + +static int clk_master_prepare(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + + spin_lock_irqsave(master->lock, flags); + + while (!clk_master_ready(master)) + cpu_relax(); + + spin_unlock_irqrestore(master->lock, flags); + + return 0; +} + +static int clk_master_is_prepared(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + bool status; + + spin_lock_irqsave(master->lock, flags); + status = clk_master_ready(master); + spin_unlock_irqrestore(master->lock, flags); + + return status; +} + +static unsigned long clk_master_div_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + u8 div; + unsigned long flags, rate = parent_rate; + struct clk_master *master = to_clk_master(hw); + const struct clk_master_layout *layout = master->layout; + const struct clk_master_characteristics *characteristics = + master->characteristics; + unsigned int mckr; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &mckr); + spin_unlock_irqrestore(master->lock, flags); + + mckr &= layout->mask; + + div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + + rate /= characteristics->divisors[div]; + + 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"); + + return rate; +} + +static int clk_master_div_save_context(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + struct clk_hw *parent_hw = clk_hw_get_parent(hw); + unsigned long flags; + unsigned int mckr, div; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &mckr); + spin_unlock_irqrestore(master->lock, flags); + + mckr &= master->layout->mask; + div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + div = master->characteristics->divisors[div]; + + master->pms.parent_rate = clk_hw_get_rate(parent_hw); + master->pms.rate = DIV_ROUND_CLOSEST(master->pms.parent_rate, div); + + return 0; +} + +static void clk_master_div_restore_context(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + unsigned int mckr; + u8 div; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &mckr); + spin_unlock_irqrestore(master->lock, flags); + + mckr &= master->layout->mask; + div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + div = master->characteristics->divisors[div]; + + if (div != DIV_ROUND_CLOSEST(master->pms.parent_rate, master->pms.rate)) + pr_warn("MCKR DIV not configured properly by firmware!\n"); +} + +static const struct clk_ops master_div_ops = { + .prepare = clk_master_prepare, + .is_prepared = clk_master_is_prepared, + .recalc_rate = clk_master_div_recalc_rate, + .save_context = clk_master_div_save_context, + .restore_context = clk_master_div_restore_context, +}; + +/* This function must be called with lock acquired. */ +static int clk_master_div_set(struct clk_master *master, + unsigned long parent_rate, int div) +{ + const struct clk_master_characteristics *characteristics = + master->characteristics; + unsigned long rate = parent_rate; + unsigned int max_div = 0, div_index = 0, max_div_index = 0; + unsigned int i, mckr, tmp; + int ret; + + for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { + if (!characteristics->divisors[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 (div > max_div) + div_index = max_div_index; + + ret = regmap_read(master->regmap, master->layout->offset, &mckr); + if (ret) + return ret; + + mckr &= master->layout->mask; + tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; + 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_index << MASTER_DIV_SHIFT); + ret = regmap_write(master->regmap, master->layout->offset, mckr); + if (ret) + return ret; + + while (!clk_master_ready(master)) + cpu_relax(); + + master->div = characteristics->divisors[div_index]; + + return 0; +} + +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); + + 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; + + 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"); +} + +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_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, + long *best_rate, + long *best_diff, + u32 div) +{ + unsigned long tmp_rate, tmp_diff; + + if (div == MASTER_PRES_MAX) + tmp_rate = parent_rate / 3; + else + tmp_rate = parent_rate >> div; + + tmp_diff = abs(req->rate - tmp_rate); + + if (*best_diff < 0 || *best_diff >= tmp_diff) { + *best_rate = tmp_rate; + *best_diff = tmp_diff; + req->best_parent_rate = parent_rate; + req->best_parent_hw = parent; + } +} + +static unsigned long clk_master_pres_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct clk_master *master = to_clk_master(hw); + const struct clk_master_characteristics *characteristics = + master->characteristics; + unsigned long flags; + unsigned int val, pres; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &val); + spin_unlock_irqrestore(master->lock, flags); + + val &= master->layout->mask; + pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; + if (pres == MASTER_PRES_MAX && characteristics->have_div3_pres) + pres = 3; + else + pres = (1 << pres); + + return DIV_ROUND_CLOSEST_ULL(parent_rate, pres); +} + +static u8 clk_master_pres_get_parent(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + unsigned int mckr; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &mckr); + spin_unlock_irqrestore(master->lock, flags); + + mckr &= master->layout->mask; + + return mckr & AT91_PMC_CSS; +} + +static int clk_master_pres_save_context(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + struct clk_hw *parent_hw = clk_hw_get_parent(hw); + unsigned long flags; + unsigned int val, pres; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &val); + spin_unlock_irqrestore(master->lock, flags); + + val &= master->layout->mask; + pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; + if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) + pres = 3; + else + pres = (1 << pres); + + master->pms.parent = val & AT91_PMC_CSS; + master->pms.parent_rate = clk_hw_get_rate(parent_hw); + master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres); + + return 0; +} + +static void clk_master_pres_restore_context(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + unsigned int val, pres; + + spin_lock_irqsave(master->lock, flags); + regmap_read(master->regmap, master->layout->offset, &val); + spin_unlock_irqrestore(master->lock, flags); + + val &= master->layout->mask; + pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; + if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) + pres = 3; + else + pres = (1 << pres); + + if (master->pms.rate != + DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres) || + (master->pms.parent != (val & AT91_PMC_CSS))) + pr_warn("MCKR PRES was not configured properly by firmware!\n"); +} + +static const struct clk_ops master_pres_ops = { + .prepare = clk_master_prepare, + .is_prepared = clk_master_is_prepared, + .recalc_rate = clk_master_pres_recalc_rate, + .get_parent = clk_master_pres_get_parent, + .save_context = clk_master_pres_save_context, + .restore_context = clk_master_pres_restore_context, +}; + +static struct clk_hw * __init +at91_clk_register_master_internal(struct regmap *regmap, + const char *name, int num_parents, + const char **parent_names, + const struct clk_master_layout *layout, + const struct clk_master_characteristics *characteristics, + const struct clk_ops *ops, spinlock_t *lock, u32 flags) +{ + 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) + return ERR_PTR(-EINVAL); + + master = kzalloc(sizeof(*master), GFP_KERNEL); + if (!master) + return ERR_PTR(-ENOMEM); + + init.name = name; + init.ops = ops; + init.parent_names = parent_names; + init.num_parents = num_parents; + init.flags = flags; + + master->hw.init = &init; + master->layout = layout; + master->characteristics = characteristics; + master->regmap = regmap; + 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) { + kfree(master); + hw = ERR_PTR(ret); + } + + return hw; +} + +struct clk_hw * __init +at91_clk_register_master_pres(struct regmap *regmap, + const char *name, int num_parents, + const char **parent_names, + const struct clk_master_layout *layout, + const struct clk_master_characteristics *characteristics, + spinlock_t *lock) +{ + return at91_clk_register_master_internal(regmap, name, num_parents, + parent_names, layout, + characteristics, + &master_pres_ops, + lock, CLK_SET_RATE_GATE); +} + +struct clk_hw * __init +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, 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; + + hw = at91_clk_register_master_internal(regmap, name, 1, + &parent_name, layout, + characteristics, ops, + lock, flags); + + 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 +clk_sama7g5_master_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct clk_master *master = to_clk_master(hw); + + return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div)); +} + +static int clk_sama7g5_master_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct clk_master *master = to_clk_master(hw); + struct clk_hw *parent; + long best_rate = LONG_MIN, best_diff = LONG_MIN; + unsigned long parent_rate; + unsigned int div, i; + + /* First: check the dividers of MCR. */ + for (i = 0; i < clk_hw_get_num_parents(hw); i++) { + parent = clk_hw_get_parent_by_index(hw, i); + if (!parent) + continue; + + parent_rate = clk_hw_get_rate(parent); + if (!parent_rate) + continue; + + for (div = 0; div < MASTER_PRES_MAX + 1; div++) { + clk_sama7g5_master_best_diff(req, parent, parent_rate, + &best_rate, &best_diff, + div); + if (!best_diff) + break; + } + + if (!best_diff) + break; + } + + /* Second: try to request rate form changeable parent. */ + if (master->chg_pid < 0) + goto end; + + parent = clk_hw_get_parent_by_index(hw, master->chg_pid); + if (!parent) + goto end; + + for (div = 0; div < MASTER_PRES_MAX + 1; div++) { + struct clk_rate_request req_parent; + unsigned long req_rate; + + if (div == MASTER_PRES_MAX) + req_rate = req->rate * 3; + else + req_rate = req->rate << div; + + clk_hw_forward_rate_request(hw, req, parent, &req_parent, req_rate); + if (__clk_determine_rate(parent, &req_parent)) + continue; + + clk_sama7g5_master_best_diff(req, parent, req_parent.rate, + &best_rate, &best_diff, div); + + if (!best_diff) + break; + } + +end: + pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n", + __func__, best_rate, + __clk_get_name((req->best_parent_hw)->clk), + req->best_parent_rate); + + if (best_rate < 0) + return -EINVAL; + + req->rate = best_rate; + + return 0; +} + +static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + u8 index; + + spin_lock_irqsave(master->lock, flags); + index = clk_mux_val_to_index(&master->hw, master->mux_table, 0, + master->parent); + spin_unlock_irqrestore(master->lock, flags); + + return index; +} + +static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + + if (index >= clk_hw_get_num_parents(hw)) + return -EINVAL; + + spin_lock_irqsave(master->lock, flags); + master->parent = clk_mux_index_to_val(master->mux_table, 0, index); + spin_unlock_irqrestore(master->lock, flags); + + return 0; +} + +static void clk_sama7g5_master_set(struct clk_master *master, + unsigned int status) +{ + unsigned long flags; + unsigned int val, cparent; + unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0; + unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT; + unsigned int div = master->div << MASTER_DIV_SHIFT; + + spin_lock_irqsave(master->lock, flags); + + regmap_write(master->regmap, AT91_PMC_MCR_V2, + AT91_PMC_MCR_V2_ID(master->id)); + regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); + regmap_update_bits(master->regmap, AT91_PMC_MCR_V2, + enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV | + AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK, + enable | parent | div | AT91_PMC_MCR_V2_CMD | + AT91_PMC_MCR_V2_ID(master->id)); + + cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; + + /* Wait here only if parent is being changed. */ + while ((cparent != master->parent) && !clk_master_ready(master)) + cpu_relax(); + + spin_unlock_irqrestore(master->lock, flags); +} + +static int clk_sama7g5_master_enable(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + + clk_sama7g5_master_set(master, 1); + + return 0; +} + +static void clk_sama7g5_master_disable(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + + spin_lock_irqsave(master->lock, flags); + + regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); + regmap_update_bits(master->regmap, AT91_PMC_MCR_V2, + AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD | + AT91_PMC_MCR_V2_ID_MSK, + AT91_PMC_MCR_V2_CMD | + AT91_PMC_MCR_V2_ID(master->id)); + + spin_unlock_irqrestore(master->lock, flags); +} + +static int clk_sama7g5_master_is_enabled(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long flags; + unsigned int val; + + spin_lock_irqsave(master->lock, flags); + + regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); + regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); + + spin_unlock_irqrestore(master->lock, flags); + + return !!(val & AT91_PMC_MCR_V2_EN); +} + +static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct clk_master *master = to_clk_master(hw); + unsigned long div, flags; + + div = DIV_ROUND_CLOSEST(parent_rate, rate); + if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1))) + return -EINVAL; + + if (div == 3) + div = MASTER_PRES_MAX; + else if (div) + div = ffs(div) - 1; + + spin_lock_irqsave(master->lock, flags); + master->div = div; + spin_unlock_irqrestore(master->lock, flags); + + return 0; +} + +static int clk_sama7g5_master_save_context(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + + master->pms.status = clk_sama7g5_master_is_enabled(hw); + + return 0; +} + +static void clk_sama7g5_master_restore_context(struct clk_hw *hw) +{ + struct clk_master *master = to_clk_master(hw); + + if (master->pms.status) + clk_sama7g5_master_set(master, master->pms.status); +} + +static const struct clk_ops sama7g5_master_ops = { + .enable = clk_sama7g5_master_enable, + .disable = clk_sama7g5_master_disable, + .is_enabled = clk_sama7g5_master_is_enabled, + .recalc_rate = clk_sama7g5_master_recalc_rate, + .determine_rate = clk_sama7g5_master_determine_rate, + .set_rate = clk_sama7g5_master_set_rate, + .get_parent = clk_sama7g5_master_get_parent, + .set_parent = clk_sama7g5_master_set_parent, + .save_context = clk_sama7g5_master_save_context, + .restore_context = clk_sama7g5_master_restore_context, +}; + +struct clk_hw * __init +at91_clk_sama7g5_register_master(struct regmap *regmap, + const char *name, int num_parents, + const char **parent_names, + u32 *mux_table, + spinlock_t *lock, u8 id, + bool critical, int chg_pid) +{ + struct clk_master *master; + struct clk_hw *hw; + struct clk_init_data init; + unsigned long flags; + unsigned int val; + int ret; + + if (!name || !num_parents || !parent_names || !mux_table || + !lock || id > MASTER_MAX_ID) + return ERR_PTR(-EINVAL); + + master = kzalloc(sizeof(*master), GFP_KERNEL); + if (!master) + return ERR_PTR(-ENOMEM); + + init.name = name; + init.ops = &sama7g5_master_ops; + init.parent_names = parent_names; + init.num_parents = num_parents; + init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; + if (chg_pid >= 0) + init.flags |= CLK_SET_RATE_PARENT; + if (critical) + init.flags |= CLK_IS_CRITICAL; + + master->hw.init = &init; + master->regmap = regmap; + master->id = id; + master->chg_pid = chg_pid; + master->lock = lock; + master->mux_table = mux_table; + + spin_lock_irqsave(master->lock, flags); + regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); + regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); + master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; + master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT; + spin_unlock_irqrestore(master->lock, flags); + + hw = &master->hw; + ret = clk_hw_register(NULL, &master->hw); + if (ret) { + kfree(master); + hw = ERR_PTR(ret); + } + + return hw; +} + +const struct clk_master_layout at91rm9200_master_layout = { + .mask = 0x31F, + .pres_shift = 2, + .offset = AT91_PMC_MCKR, +}; + +const struct clk_master_layout at91sam9x5_master_layout = { + .mask = 0x373, + .pres_shift = 4, + .offset = AT91_PMC_MCKR, +}; |