| // 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 9 | |
| #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, | |
| struct clk_hw **parent_hws, | |
| 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 || parent_hws) || !lock) | |
| return ERR_PTR(-EINVAL); | |
| master = kzalloc_obj(*master); | |
| if (!master) | |
| return ERR_PTR(-ENOMEM); | |
| init.name = name; | |
| init.ops = ops; | |
| if (parent_hws) | |
| init.parent_hws = (const struct clk_hw **)parent_hws; | |
| else | |
| 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, | |
| struct clk_hw **parent_hws, | |
| 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, parent_hws, 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, | |
| struct clk_hw *parent_hw, 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 ? &parent_name : NULL, | |
| parent_hw ? &parent_hw : NULL, 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); | |
| if (master->div == MASTER_PRES_MAX) | |
| return DIV_ROUND_CLOSEST_ULL(parent_rate, 3); | |
| 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, | |
| struct clk_hw **parent_hws, | |
| 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 || parent_hws) || !mux_table || | |
| !lock || id > MASTER_MAX_ID) | |
| return ERR_PTR(-EINVAL); | |
| master = kzalloc_obj(*master); | |
| if (!master) | |
| return ERR_PTR(-ENOMEM); | |
| init.name = name; | |
| init.ops = &sama7g5_master_ops; | |
| if (parent_hws) | |
| init.parent_hws = (const struct clk_hw **)parent_hws; | |
| else | |
| 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, | |
| }; |