12874c5fdSThomas Gleixner // SPDX-License-Identifier: GPL-2.0-or-later 2e442d234SBoris BREZILLON /* 3e442d234SBoris BREZILLON * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 4e442d234SBoris BREZILLON */ 5e442d234SBoris BREZILLON 6e442d234SBoris BREZILLON #include <linux/clk-provider.h> 7e442d234SBoris BREZILLON #include <linux/clkdev.h> 8e442d234SBoris BREZILLON #include <linux/clk/at91_pmc.h> 9e442d234SBoris BREZILLON #include <linux/of.h> 101bdf0232SBoris Brezillon #include <linux/mfd/syscon.h> 111bdf0232SBoris Brezillon #include <linux/regmap.h> 12e442d234SBoris BREZILLON 13e442d234SBoris BREZILLON #include "pmc.h" 14e442d234SBoris BREZILLON 15e442d234SBoris BREZILLON #define MASTER_PRES_MASK 0x7 16e442d234SBoris BREZILLON #define MASTER_PRES_MAX MASTER_PRES_MASK 17e442d234SBoris BREZILLON #define MASTER_DIV_SHIFT 8 18e26b3006SEugen Hristev #define MASTER_DIV_MASK 0x7 19e442d234SBoris BREZILLON 2075c88143SClaudiu Beznea #define PMC_MCR_CSS_SHIFT (16) 2175c88143SClaudiu Beznea 2275c88143SClaudiu Beznea #define MASTER_MAX_ID 4 2375c88143SClaudiu Beznea 24e442d234SBoris BREZILLON #define to_clk_master(hw) container_of(hw, struct clk_master, hw) 25e442d234SBoris BREZILLON 26e442d234SBoris BREZILLON struct clk_master { 27e442d234SBoris BREZILLON struct clk_hw hw; 281bdf0232SBoris Brezillon struct regmap *regmap; 2975c88143SClaudiu Beznea spinlock_t *lock; 30e442d234SBoris BREZILLON const struct clk_master_layout *layout; 31e442d234SBoris BREZILLON const struct clk_master_characteristics *characteristics; 3236971566SClaudiu Beznea struct at91_clk_pms pms; 3375c88143SClaudiu Beznea u32 *mux_table; 34e5be5370SAlexandre Belloni u32 mckr; 3575c88143SClaudiu Beznea int chg_pid; 3675c88143SClaudiu Beznea u8 id; 3775c88143SClaudiu Beznea u8 parent; 3875c88143SClaudiu Beznea u8 div; 39e442d234SBoris BREZILLON }; 40e442d234SBoris BREZILLON 4175c88143SClaudiu Beznea static inline bool clk_master_ready(struct clk_master *master) 421bdf0232SBoris Brezillon { 4375c88143SClaudiu Beznea unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY; 441bdf0232SBoris Brezillon unsigned int status; 451bdf0232SBoris Brezillon 4675c88143SClaudiu Beznea regmap_read(master->regmap, AT91_PMC_SR, &status); 471bdf0232SBoris Brezillon 4875c88143SClaudiu Beznea return !!(status & bit); 491bdf0232SBoris Brezillon } 501bdf0232SBoris Brezillon 51e442d234SBoris BREZILLON static int clk_master_prepare(struct clk_hw *hw) 52e442d234SBoris BREZILLON { 53e442d234SBoris BREZILLON struct clk_master *master = to_clk_master(hw); 547a110b91SClaudiu Beznea unsigned long flags; 557a110b91SClaudiu Beznea 567a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 57e442d234SBoris BREZILLON 5875c88143SClaudiu Beznea while (!clk_master_ready(master)) 5999a81706SAlexandre Belloni cpu_relax(); 60e442d234SBoris BREZILLON 617a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 627a110b91SClaudiu Beznea 63e442d234SBoris BREZILLON return 0; 64e442d234SBoris BREZILLON } 65e442d234SBoris BREZILLON 66e442d234SBoris BREZILLON static int clk_master_is_prepared(struct clk_hw *hw) 67e442d234SBoris BREZILLON { 68e442d234SBoris BREZILLON struct clk_master *master = to_clk_master(hw); 697a110b91SClaudiu Beznea unsigned long flags; 707a110b91SClaudiu Beznea bool status; 71e442d234SBoris BREZILLON 727a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 737a110b91SClaudiu Beznea status = clk_master_ready(master); 747a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 757a110b91SClaudiu Beznea 767a110b91SClaudiu Beznea return status; 77e442d234SBoris BREZILLON } 78e442d234SBoris BREZILLON 797a110b91SClaudiu Beznea static unsigned long clk_master_div_recalc_rate(struct clk_hw *hw, 80e442d234SBoris BREZILLON unsigned long parent_rate) 81e442d234SBoris BREZILLON { 82e442d234SBoris BREZILLON u8 div; 837a110b91SClaudiu Beznea unsigned long flags, rate = parent_rate; 84e442d234SBoris BREZILLON struct clk_master *master = to_clk_master(hw); 85e442d234SBoris BREZILLON const struct clk_master_layout *layout = master->layout; 86e442d234SBoris BREZILLON const struct clk_master_characteristics *characteristics = 87e442d234SBoris BREZILLON master->characteristics; 881bdf0232SBoris Brezillon unsigned int mckr; 89e442d234SBoris BREZILLON 907a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 91e5be5370SAlexandre Belloni regmap_read(master->regmap, master->layout->offset, &mckr); 927a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 937a110b91SClaudiu Beznea 941bdf0232SBoris Brezillon mckr &= layout->mask; 95e442d234SBoris BREZILLON 961bdf0232SBoris Brezillon div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; 97e442d234SBoris BREZILLON 98e442d234SBoris BREZILLON rate /= characteristics->divisors[div]; 99e442d234SBoris BREZILLON 100e442d234SBoris BREZILLON if (rate < characteristics->output.min) 1017a110b91SClaudiu Beznea pr_warn("master clk div is underclocked"); 102e442d234SBoris BREZILLON else if (rate > characteristics->output.max) 1037a110b91SClaudiu Beznea pr_warn("master clk div is overclocked"); 104e442d234SBoris BREZILLON 105e442d234SBoris BREZILLON return rate; 106e442d234SBoris BREZILLON } 107e442d234SBoris BREZILLON 10836971566SClaudiu Beznea static int clk_master_div_save_context(struct clk_hw *hw) 10936971566SClaudiu Beznea { 11036971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 11136971566SClaudiu Beznea struct clk_hw *parent_hw = clk_hw_get_parent(hw); 11236971566SClaudiu Beznea unsigned long flags; 11336971566SClaudiu Beznea unsigned int mckr, div; 11436971566SClaudiu Beznea 11536971566SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 11636971566SClaudiu Beznea regmap_read(master->regmap, master->layout->offset, &mckr); 11736971566SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 11836971566SClaudiu Beznea 11936971566SClaudiu Beznea mckr &= master->layout->mask; 12036971566SClaudiu Beznea div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; 12136971566SClaudiu Beznea div = master->characteristics->divisors[div]; 12236971566SClaudiu Beznea 12336971566SClaudiu Beznea master->pms.parent_rate = clk_hw_get_rate(parent_hw); 12436971566SClaudiu Beznea master->pms.rate = DIV_ROUND_CLOSEST(master->pms.parent_rate, div); 12536971566SClaudiu Beznea 12636971566SClaudiu Beznea return 0; 12736971566SClaudiu Beznea } 12836971566SClaudiu Beznea 12936971566SClaudiu Beznea static void clk_master_div_restore_context(struct clk_hw *hw) 13036971566SClaudiu Beznea { 13136971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 13236971566SClaudiu Beznea unsigned long flags; 13336971566SClaudiu Beznea unsigned int mckr; 13436971566SClaudiu Beznea u8 div; 13536971566SClaudiu Beznea 13636971566SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 13736971566SClaudiu Beznea regmap_read(master->regmap, master->layout->offset, &mckr); 13836971566SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 13936971566SClaudiu Beznea 14036971566SClaudiu Beznea mckr &= master->layout->mask; 14136971566SClaudiu Beznea div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; 14236971566SClaudiu Beznea div = master->characteristics->divisors[div]; 14336971566SClaudiu Beznea 14436971566SClaudiu Beznea if (div != DIV_ROUND_CLOSEST(master->pms.parent_rate, master->pms.rate)) 14536971566SClaudiu Beznea pr_warn("MCKR DIV not configured properly by firmware!\n"); 14636971566SClaudiu Beznea } 14736971566SClaudiu Beznea 1487a110b91SClaudiu Beznea static const struct clk_ops master_div_ops = { 149e442d234SBoris BREZILLON .prepare = clk_master_prepare, 150e442d234SBoris BREZILLON .is_prepared = clk_master_is_prepared, 1517a110b91SClaudiu Beznea .recalc_rate = clk_master_div_recalc_rate, 15236971566SClaudiu Beznea .save_context = clk_master_div_save_context, 15336971566SClaudiu Beznea .restore_context = clk_master_div_restore_context, 154e442d234SBoris BREZILLON }; 155e442d234SBoris BREZILLON 1567a110b91SClaudiu Beznea static int clk_master_div_set_rate(struct clk_hw *hw, unsigned long rate, 15775c88143SClaudiu Beznea unsigned long parent_rate) 15875c88143SClaudiu Beznea { 15975c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 1607a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics = 1617a110b91SClaudiu Beznea master->characteristics; 1627a110b91SClaudiu Beznea unsigned long flags; 16336971566SClaudiu Beznea unsigned int mckr, tmp; 1647a110b91SClaudiu Beznea int div, i; 16536971566SClaudiu Beznea int ret; 16675c88143SClaudiu Beznea 1677a110b91SClaudiu Beznea div = DIV_ROUND_CLOSEST(parent_rate, rate); 1687a110b91SClaudiu Beznea if (div > ARRAY_SIZE(characteristics->divisors)) 1697a110b91SClaudiu Beznea return -EINVAL; 1707a110b91SClaudiu Beznea 1717a110b91SClaudiu Beznea for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { 1727a110b91SClaudiu Beznea if (!characteristics->divisors[i]) 1737a110b91SClaudiu Beznea break; 1747a110b91SClaudiu Beznea 1757a110b91SClaudiu Beznea if (div == characteristics->divisors[i]) { 1767a110b91SClaudiu Beznea div = i; 1777a110b91SClaudiu Beznea break; 17875c88143SClaudiu Beznea } 1797a110b91SClaudiu Beznea } 1807a110b91SClaudiu Beznea 1817a110b91SClaudiu Beznea if (i == ARRAY_SIZE(characteristics->divisors)) 1827a110b91SClaudiu Beznea return -EINVAL; 1837a110b91SClaudiu Beznea 1847a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 18536971566SClaudiu Beznea ret = regmap_read(master->regmap, master->layout->offset, &mckr); 18636971566SClaudiu Beznea if (ret) 18736971566SClaudiu Beznea goto unlock; 18836971566SClaudiu Beznea 189*a27748adSClaudiu Beznea mckr &= master->layout->mask; 190*a27748adSClaudiu Beznea tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; 19136971566SClaudiu Beznea if (tmp == div) 19236971566SClaudiu Beznea goto unlock; 19336971566SClaudiu Beznea 19436971566SClaudiu Beznea mckr &= ~(MASTER_DIV_MASK << MASTER_DIV_SHIFT); 19536971566SClaudiu Beznea mckr |= (div << MASTER_DIV_SHIFT); 19636971566SClaudiu Beznea ret = regmap_write(master->regmap, master->layout->offset, mckr); 19736971566SClaudiu Beznea if (ret) 19836971566SClaudiu Beznea goto unlock; 19936971566SClaudiu Beznea 2007a110b91SClaudiu Beznea while (!clk_master_ready(master)) 2017a110b91SClaudiu Beznea cpu_relax(); 20236971566SClaudiu Beznea unlock: 2037a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 2047a110b91SClaudiu Beznea 2057a110b91SClaudiu Beznea return 0; 2067a110b91SClaudiu Beznea } 2077a110b91SClaudiu Beznea 2087a110b91SClaudiu Beznea static int clk_master_div_determine_rate(struct clk_hw *hw, 2097a110b91SClaudiu Beznea struct clk_rate_request *req) 2107a110b91SClaudiu Beznea { 2117a110b91SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 2127a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics = 2137a110b91SClaudiu Beznea master->characteristics; 2147a110b91SClaudiu Beznea struct clk_hw *parent; 2157a110b91SClaudiu Beznea unsigned long parent_rate, tmp_rate, best_rate = 0; 2167a110b91SClaudiu Beznea int i, best_diff = INT_MIN, tmp_diff; 2177a110b91SClaudiu Beznea 2187a110b91SClaudiu Beznea parent = clk_hw_get_parent(hw); 2197a110b91SClaudiu Beznea if (!parent) 2207a110b91SClaudiu Beznea return -EINVAL; 2217a110b91SClaudiu Beznea 2227a110b91SClaudiu Beznea parent_rate = clk_hw_get_rate(parent); 2237a110b91SClaudiu Beznea if (!parent_rate) 2247a110b91SClaudiu Beznea return -EINVAL; 2257a110b91SClaudiu Beznea 2267a110b91SClaudiu Beznea for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { 2277a110b91SClaudiu Beznea if (!characteristics->divisors[i]) 2287a110b91SClaudiu Beznea break; 2297a110b91SClaudiu Beznea 2307a110b91SClaudiu Beznea tmp_rate = DIV_ROUND_CLOSEST_ULL(parent_rate, 2317a110b91SClaudiu Beznea characteristics->divisors[i]); 2327a110b91SClaudiu Beznea tmp_diff = abs(tmp_rate - req->rate); 2337a110b91SClaudiu Beznea 2347a110b91SClaudiu Beznea if (!best_rate || best_diff > tmp_diff) { 2357a110b91SClaudiu Beznea best_diff = tmp_diff; 2367a110b91SClaudiu Beznea best_rate = tmp_rate; 2377a110b91SClaudiu Beznea } 2387a110b91SClaudiu Beznea 2397a110b91SClaudiu Beznea if (!best_diff) 2407a110b91SClaudiu Beznea break; 2417a110b91SClaudiu Beznea } 2427a110b91SClaudiu Beznea 2437a110b91SClaudiu Beznea req->best_parent_rate = best_rate; 2447a110b91SClaudiu Beznea req->best_parent_hw = parent; 2457a110b91SClaudiu Beznea req->rate = best_rate; 2467a110b91SClaudiu Beznea 2477a110b91SClaudiu Beznea return 0; 2487a110b91SClaudiu Beznea } 2497a110b91SClaudiu Beznea 25036971566SClaudiu Beznea static void clk_master_div_restore_context_chg(struct clk_hw *hw) 25136971566SClaudiu Beznea { 25236971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 25336971566SClaudiu Beznea int ret; 25436971566SClaudiu Beznea 25536971566SClaudiu Beznea ret = clk_master_div_set_rate(hw, master->pms.rate, 25636971566SClaudiu Beznea master->pms.parent_rate); 25736971566SClaudiu Beznea if (ret) 25836971566SClaudiu Beznea pr_warn("Failed to restore MCK DIV clock\n"); 25936971566SClaudiu Beznea } 26036971566SClaudiu Beznea 2617a110b91SClaudiu Beznea static const struct clk_ops master_div_ops_chg = { 2627a110b91SClaudiu Beznea .prepare = clk_master_prepare, 2637a110b91SClaudiu Beznea .is_prepared = clk_master_is_prepared, 2647a110b91SClaudiu Beznea .recalc_rate = clk_master_div_recalc_rate, 2657a110b91SClaudiu Beznea .determine_rate = clk_master_div_determine_rate, 2667a110b91SClaudiu Beznea .set_rate = clk_master_div_set_rate, 26736971566SClaudiu Beznea .save_context = clk_master_div_save_context, 26836971566SClaudiu Beznea .restore_context = clk_master_div_restore_context_chg, 2697a110b91SClaudiu Beznea }; 27075c88143SClaudiu Beznea 27175c88143SClaudiu Beznea static void clk_sama7g5_master_best_diff(struct clk_rate_request *req, 27275c88143SClaudiu Beznea struct clk_hw *parent, 27375c88143SClaudiu Beznea unsigned long parent_rate, 27475c88143SClaudiu Beznea long *best_rate, 27575c88143SClaudiu Beznea long *best_diff, 27675c88143SClaudiu Beznea u32 div) 27775c88143SClaudiu Beznea { 27875c88143SClaudiu Beznea unsigned long tmp_rate, tmp_diff; 27975c88143SClaudiu Beznea 28075c88143SClaudiu Beznea if (div == MASTER_PRES_MAX) 28175c88143SClaudiu Beznea tmp_rate = parent_rate / 3; 28275c88143SClaudiu Beznea else 28375c88143SClaudiu Beznea tmp_rate = parent_rate >> div; 28475c88143SClaudiu Beznea 28575c88143SClaudiu Beznea tmp_diff = abs(req->rate - tmp_rate); 28675c88143SClaudiu Beznea 28775c88143SClaudiu Beznea if (*best_diff < 0 || *best_diff >= tmp_diff) { 28875c88143SClaudiu Beznea *best_rate = tmp_rate; 28975c88143SClaudiu Beznea *best_diff = tmp_diff; 29075c88143SClaudiu Beznea req->best_parent_rate = parent_rate; 29175c88143SClaudiu Beznea req->best_parent_hw = parent; 29275c88143SClaudiu Beznea } 29375c88143SClaudiu Beznea } 29475c88143SClaudiu Beznea 2957a110b91SClaudiu Beznea static int clk_master_pres_determine_rate(struct clk_hw *hw, 2967a110b91SClaudiu Beznea struct clk_rate_request *req) 2977a110b91SClaudiu Beznea { 2987a110b91SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 2997a110b91SClaudiu Beznea struct clk_rate_request req_parent = *req; 3007a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics = 3017a110b91SClaudiu Beznea master->characteristics; 3027a110b91SClaudiu Beznea struct clk_hw *parent; 3037a110b91SClaudiu Beznea long best_rate = LONG_MIN, best_diff = LONG_MIN; 3047a110b91SClaudiu Beznea u32 pres; 3057a110b91SClaudiu Beznea int i; 3067a110b91SClaudiu Beznea 3077a110b91SClaudiu Beznea if (master->chg_pid < 0) 3087a110b91SClaudiu Beznea return -EOPNOTSUPP; 3097a110b91SClaudiu Beznea 3107a110b91SClaudiu Beznea parent = clk_hw_get_parent_by_index(hw, master->chg_pid); 3117a110b91SClaudiu Beznea if (!parent) 3127a110b91SClaudiu Beznea return -EOPNOTSUPP; 3137a110b91SClaudiu Beznea 3147a110b91SClaudiu Beznea for (i = 0; i <= MASTER_PRES_MAX; i++) { 3157a110b91SClaudiu Beznea if (characteristics->have_div3_pres && i == MASTER_PRES_MAX) 3167a110b91SClaudiu Beznea pres = 3; 3177a110b91SClaudiu Beznea else 3187a110b91SClaudiu Beznea pres = 1 << i; 3197a110b91SClaudiu Beznea 3207a110b91SClaudiu Beznea req_parent.rate = req->rate * pres; 3217a110b91SClaudiu Beznea if (__clk_determine_rate(parent, &req_parent)) 3227a110b91SClaudiu Beznea continue; 3237a110b91SClaudiu Beznea 3247a110b91SClaudiu Beznea clk_sama7g5_master_best_diff(req, parent, req_parent.rate, 3257a110b91SClaudiu Beznea &best_diff, &best_rate, pres); 3267a110b91SClaudiu Beznea if (!best_diff) 3277a110b91SClaudiu Beznea break; 3287a110b91SClaudiu Beznea } 3297a110b91SClaudiu Beznea 3307a110b91SClaudiu Beznea return 0; 3317a110b91SClaudiu Beznea } 3327a110b91SClaudiu Beznea 3337a110b91SClaudiu Beznea static int clk_master_pres_set_rate(struct clk_hw *hw, unsigned long rate, 3347a110b91SClaudiu Beznea unsigned long parent_rate) 3357a110b91SClaudiu Beznea { 3367a110b91SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 3377a110b91SClaudiu Beznea unsigned long flags; 33836971566SClaudiu Beznea unsigned int pres, mckr, tmp; 33936971566SClaudiu Beznea int ret; 3407a110b91SClaudiu Beznea 3417a110b91SClaudiu Beznea pres = DIV_ROUND_CLOSEST(parent_rate, rate); 3427a110b91SClaudiu Beznea if (pres > MASTER_PRES_MAX) 3437a110b91SClaudiu Beznea return -EINVAL; 3447a110b91SClaudiu Beznea 3457a110b91SClaudiu Beznea else if (pres == 3) 3467a110b91SClaudiu Beznea pres = MASTER_PRES_MAX; 347c2910c00SClaudiu Beznea else if (pres) 3487a110b91SClaudiu Beznea pres = ffs(pres) - 1; 3497a110b91SClaudiu Beznea 3507a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 35136971566SClaudiu Beznea ret = regmap_read(master->regmap, master->layout->offset, &mckr); 35236971566SClaudiu Beznea if (ret) 35336971566SClaudiu Beznea goto unlock; 35436971566SClaudiu Beznea 35536971566SClaudiu Beznea mckr &= master->layout->mask; 35636971566SClaudiu Beznea tmp = (mckr >> master->layout->pres_shift) & MASTER_PRES_MASK; 35736971566SClaudiu Beznea if (pres == tmp) 35836971566SClaudiu Beznea goto unlock; 35936971566SClaudiu Beznea 36036971566SClaudiu Beznea mckr &= ~(MASTER_PRES_MASK << master->layout->pres_shift); 36136971566SClaudiu Beznea mckr |= (pres << master->layout->pres_shift); 36236971566SClaudiu Beznea ret = regmap_write(master->regmap, master->layout->offset, mckr); 36336971566SClaudiu Beznea if (ret) 36436971566SClaudiu Beznea goto unlock; 3657a110b91SClaudiu Beznea 3667a110b91SClaudiu Beznea while (!clk_master_ready(master)) 3677a110b91SClaudiu Beznea cpu_relax(); 36836971566SClaudiu Beznea unlock: 3697a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 3707a110b91SClaudiu Beznea 37136971566SClaudiu Beznea return ret; 3727a110b91SClaudiu Beznea } 3737a110b91SClaudiu Beznea 3747a110b91SClaudiu Beznea static unsigned long clk_master_pres_recalc_rate(struct clk_hw *hw, 3757a110b91SClaudiu Beznea unsigned long parent_rate) 3767a110b91SClaudiu Beznea { 3777a110b91SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 3787a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics = 3797a110b91SClaudiu Beznea master->characteristics; 3807a110b91SClaudiu Beznea unsigned long flags; 3817a110b91SClaudiu Beznea unsigned int val, pres; 3827a110b91SClaudiu Beznea 3837a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 3847a110b91SClaudiu Beznea regmap_read(master->regmap, master->layout->offset, &val); 3857a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 3867a110b91SClaudiu Beznea 387*a27748adSClaudiu Beznea val &= master->layout->mask; 3887a110b91SClaudiu Beznea pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 3897a110b91SClaudiu Beznea if (pres == 3 && characteristics->have_div3_pres) 3907a110b91SClaudiu Beznea pres = 3; 3917a110b91SClaudiu Beznea else 3927a110b91SClaudiu Beznea pres = (1 << pres); 3937a110b91SClaudiu Beznea 3947a110b91SClaudiu Beznea return DIV_ROUND_CLOSEST_ULL(parent_rate, pres); 3957a110b91SClaudiu Beznea } 3967a110b91SClaudiu Beznea 3977a110b91SClaudiu Beznea static u8 clk_master_pres_get_parent(struct clk_hw *hw) 3987a110b91SClaudiu Beznea { 3997a110b91SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 4007a110b91SClaudiu Beznea unsigned long flags; 4017a110b91SClaudiu Beznea unsigned int mckr; 4027a110b91SClaudiu Beznea 4037a110b91SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 4047a110b91SClaudiu Beznea regmap_read(master->regmap, master->layout->offset, &mckr); 4057a110b91SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 4067a110b91SClaudiu Beznea 407*a27748adSClaudiu Beznea mckr &= master->layout->mask; 408*a27748adSClaudiu Beznea 4097a110b91SClaudiu Beznea return mckr & AT91_PMC_CSS; 4107a110b91SClaudiu Beznea } 4117a110b91SClaudiu Beznea 41236971566SClaudiu Beznea static int clk_master_pres_save_context(struct clk_hw *hw) 41336971566SClaudiu Beznea { 41436971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 41536971566SClaudiu Beznea struct clk_hw *parent_hw = clk_hw_get_parent(hw); 41636971566SClaudiu Beznea unsigned long flags; 41736971566SClaudiu Beznea unsigned int val, pres; 41836971566SClaudiu Beznea 41936971566SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 42036971566SClaudiu Beznea regmap_read(master->regmap, master->layout->offset, &val); 42136971566SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 42236971566SClaudiu Beznea 42336971566SClaudiu Beznea val &= master->layout->mask; 42436971566SClaudiu Beznea pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 42536971566SClaudiu Beznea if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) 42636971566SClaudiu Beznea pres = 3; 42736971566SClaudiu Beznea else 42836971566SClaudiu Beznea pres = (1 << pres); 42936971566SClaudiu Beznea 43036971566SClaudiu Beznea master->pms.parent = val & AT91_PMC_CSS; 43136971566SClaudiu Beznea master->pms.parent_rate = clk_hw_get_rate(parent_hw); 43236971566SClaudiu Beznea master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres); 43336971566SClaudiu Beznea 43436971566SClaudiu Beznea return 0; 43536971566SClaudiu Beznea } 43636971566SClaudiu Beznea 43736971566SClaudiu Beznea static void clk_master_pres_restore_context(struct clk_hw *hw) 43836971566SClaudiu Beznea { 43936971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 44036971566SClaudiu Beznea unsigned long flags; 44136971566SClaudiu Beznea unsigned int val, pres; 44236971566SClaudiu Beznea 44336971566SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 44436971566SClaudiu Beznea regmap_read(master->regmap, master->layout->offset, &val); 44536971566SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 44636971566SClaudiu Beznea 44736971566SClaudiu Beznea val &= master->layout->mask; 44836971566SClaudiu Beznea pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 44936971566SClaudiu Beznea if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) 45036971566SClaudiu Beznea pres = 3; 45136971566SClaudiu Beznea else 45236971566SClaudiu Beznea pres = (1 << pres); 45336971566SClaudiu Beznea 45436971566SClaudiu Beznea if (master->pms.rate != 45536971566SClaudiu Beznea DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres) || 45636971566SClaudiu Beznea (master->pms.parent != (val & AT91_PMC_CSS))) 45736971566SClaudiu Beznea pr_warn("MCKR PRES was not configured properly by firmware!\n"); 45836971566SClaudiu Beznea } 45936971566SClaudiu Beznea 46036971566SClaudiu Beznea static void clk_master_pres_restore_context_chg(struct clk_hw *hw) 46136971566SClaudiu Beznea { 46236971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 46336971566SClaudiu Beznea 46436971566SClaudiu Beznea clk_master_pres_set_rate(hw, master->pms.rate, master->pms.parent_rate); 46536971566SClaudiu Beznea } 46636971566SClaudiu Beznea 4677a110b91SClaudiu Beznea static const struct clk_ops master_pres_ops = { 4687a110b91SClaudiu Beznea .prepare = clk_master_prepare, 4697a110b91SClaudiu Beznea .is_prepared = clk_master_is_prepared, 4707a110b91SClaudiu Beznea .recalc_rate = clk_master_pres_recalc_rate, 4717a110b91SClaudiu Beznea .get_parent = clk_master_pres_get_parent, 47236971566SClaudiu Beznea .save_context = clk_master_pres_save_context, 47336971566SClaudiu Beznea .restore_context = clk_master_pres_restore_context, 4747a110b91SClaudiu Beznea }; 4757a110b91SClaudiu Beznea 4767a110b91SClaudiu Beznea static const struct clk_ops master_pres_ops_chg = { 4777a110b91SClaudiu Beznea .prepare = clk_master_prepare, 4787a110b91SClaudiu Beznea .is_prepared = clk_master_is_prepared, 4797a110b91SClaudiu Beznea .determine_rate = clk_master_pres_determine_rate, 4807a110b91SClaudiu Beznea .recalc_rate = clk_master_pres_recalc_rate, 4817a110b91SClaudiu Beznea .get_parent = clk_master_pres_get_parent, 4827a110b91SClaudiu Beznea .set_rate = clk_master_pres_set_rate, 48336971566SClaudiu Beznea .save_context = clk_master_pres_save_context, 48436971566SClaudiu Beznea .restore_context = clk_master_pres_restore_context_chg, 4857a110b91SClaudiu Beznea }; 4867a110b91SClaudiu Beznea 4877a110b91SClaudiu Beznea static struct clk_hw * __init 4887a110b91SClaudiu Beznea at91_clk_register_master_internal(struct regmap *regmap, 4897a110b91SClaudiu Beznea const char *name, int num_parents, 4907a110b91SClaudiu Beznea const char **parent_names, 4917a110b91SClaudiu Beznea const struct clk_master_layout *layout, 4927a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics, 4937a110b91SClaudiu Beznea const struct clk_ops *ops, spinlock_t *lock, u32 flags, 4947a110b91SClaudiu Beznea int chg_pid) 4957a110b91SClaudiu Beznea { 4967a110b91SClaudiu Beznea struct clk_master *master; 4977a110b91SClaudiu Beznea struct clk_init_data init; 4987a110b91SClaudiu Beznea struct clk_hw *hw; 4997a110b91SClaudiu Beznea int ret; 5007a110b91SClaudiu Beznea 5017a110b91SClaudiu Beznea if (!name || !num_parents || !parent_names || !lock) 5027a110b91SClaudiu Beznea return ERR_PTR(-EINVAL); 5037a110b91SClaudiu Beznea 5047a110b91SClaudiu Beznea master = kzalloc(sizeof(*master), GFP_KERNEL); 5057a110b91SClaudiu Beznea if (!master) 5067a110b91SClaudiu Beznea return ERR_PTR(-ENOMEM); 5077a110b91SClaudiu Beznea 5087a110b91SClaudiu Beznea init.name = name; 5097a110b91SClaudiu Beznea init.ops = ops; 5107a110b91SClaudiu Beznea init.parent_names = parent_names; 5117a110b91SClaudiu Beznea init.num_parents = num_parents; 5127a110b91SClaudiu Beznea init.flags = flags; 5137a110b91SClaudiu Beznea 5147a110b91SClaudiu Beznea master->hw.init = &init; 5157a110b91SClaudiu Beznea master->layout = layout; 5167a110b91SClaudiu Beznea master->characteristics = characteristics; 5177a110b91SClaudiu Beznea master->regmap = regmap; 5187a110b91SClaudiu Beznea master->chg_pid = chg_pid; 5197a110b91SClaudiu Beznea master->lock = lock; 5207a110b91SClaudiu Beznea 5217a110b91SClaudiu Beznea hw = &master->hw; 5227a110b91SClaudiu Beznea ret = clk_hw_register(NULL, &master->hw); 5237a110b91SClaudiu Beznea if (ret) { 5247a110b91SClaudiu Beznea kfree(master); 5257a110b91SClaudiu Beznea hw = ERR_PTR(ret); 5267a110b91SClaudiu Beznea } 5277a110b91SClaudiu Beznea 5287a110b91SClaudiu Beznea return hw; 5297a110b91SClaudiu Beznea } 5307a110b91SClaudiu Beznea 5317a110b91SClaudiu Beznea struct clk_hw * __init 5327a110b91SClaudiu Beznea at91_clk_register_master_pres(struct regmap *regmap, 5337a110b91SClaudiu Beznea const char *name, int num_parents, 5347a110b91SClaudiu Beznea const char **parent_names, 5357a110b91SClaudiu Beznea const struct clk_master_layout *layout, 5367a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics, 5377a110b91SClaudiu Beznea spinlock_t *lock, u32 flags, int chg_pid) 5387a110b91SClaudiu Beznea { 5397a110b91SClaudiu Beznea const struct clk_ops *ops; 5407a110b91SClaudiu Beznea 5417a110b91SClaudiu Beznea if (flags & CLK_SET_RATE_GATE) 5427a110b91SClaudiu Beznea ops = &master_pres_ops; 5437a110b91SClaudiu Beznea else 5447a110b91SClaudiu Beznea ops = &master_pres_ops_chg; 5457a110b91SClaudiu Beznea 5467a110b91SClaudiu Beznea return at91_clk_register_master_internal(regmap, name, num_parents, 5477a110b91SClaudiu Beznea parent_names, layout, 5487a110b91SClaudiu Beznea characteristics, ops, 5497a110b91SClaudiu Beznea lock, flags, chg_pid); 5507a110b91SClaudiu Beznea } 5517a110b91SClaudiu Beznea 5527a110b91SClaudiu Beznea struct clk_hw * __init 5537a110b91SClaudiu Beznea at91_clk_register_master_div(struct regmap *regmap, 5547a110b91SClaudiu Beznea const char *name, const char *parent_name, 5557a110b91SClaudiu Beznea const struct clk_master_layout *layout, 5567a110b91SClaudiu Beznea const struct clk_master_characteristics *characteristics, 5577a110b91SClaudiu Beznea spinlock_t *lock, u32 flags) 5587a110b91SClaudiu Beznea { 5597a110b91SClaudiu Beznea const struct clk_ops *ops; 5607a110b91SClaudiu Beznea 5617a110b91SClaudiu Beznea if (flags & CLK_SET_RATE_GATE) 5627a110b91SClaudiu Beznea ops = &master_div_ops; 5637a110b91SClaudiu Beznea else 5647a110b91SClaudiu Beznea ops = &master_div_ops_chg; 5657a110b91SClaudiu Beznea 5667a110b91SClaudiu Beznea return at91_clk_register_master_internal(regmap, name, 1, 5677a110b91SClaudiu Beznea &parent_name, layout, 5687a110b91SClaudiu Beznea characteristics, ops, 5697a110b91SClaudiu Beznea lock, flags, -EINVAL); 5707a110b91SClaudiu Beznea } 5717a110b91SClaudiu Beznea 5727a110b91SClaudiu Beznea static unsigned long 5737a110b91SClaudiu Beznea clk_sama7g5_master_recalc_rate(struct clk_hw *hw, 5747a110b91SClaudiu Beznea unsigned long parent_rate) 5757a110b91SClaudiu Beznea { 5767a110b91SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 5777a110b91SClaudiu Beznea 5787a110b91SClaudiu Beznea return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div)); 5797a110b91SClaudiu Beznea } 5807a110b91SClaudiu Beznea 58175c88143SClaudiu Beznea static int clk_sama7g5_master_determine_rate(struct clk_hw *hw, 58275c88143SClaudiu Beznea struct clk_rate_request *req) 58375c88143SClaudiu Beznea { 58475c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 58575c88143SClaudiu Beznea struct clk_rate_request req_parent = *req; 58675c88143SClaudiu Beznea struct clk_hw *parent; 58775c88143SClaudiu Beznea long best_rate = LONG_MIN, best_diff = LONG_MIN; 58875c88143SClaudiu Beznea unsigned long parent_rate; 58975c88143SClaudiu Beznea unsigned int div, i; 59075c88143SClaudiu Beznea 59175c88143SClaudiu Beznea /* First: check the dividers of MCR. */ 59275c88143SClaudiu Beznea for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 59375c88143SClaudiu Beznea parent = clk_hw_get_parent_by_index(hw, i); 59475c88143SClaudiu Beznea if (!parent) 59575c88143SClaudiu Beznea continue; 59675c88143SClaudiu Beznea 59775c88143SClaudiu Beznea parent_rate = clk_hw_get_rate(parent); 59875c88143SClaudiu Beznea if (!parent_rate) 59975c88143SClaudiu Beznea continue; 60075c88143SClaudiu Beznea 60175c88143SClaudiu Beznea for (div = 0; div < MASTER_PRES_MAX + 1; div++) { 60275c88143SClaudiu Beznea clk_sama7g5_master_best_diff(req, parent, parent_rate, 60375c88143SClaudiu Beznea &best_rate, &best_diff, 60475c88143SClaudiu Beznea div); 60575c88143SClaudiu Beznea if (!best_diff) 60675c88143SClaudiu Beznea break; 60775c88143SClaudiu Beznea } 60875c88143SClaudiu Beznea 60975c88143SClaudiu Beznea if (!best_diff) 61075c88143SClaudiu Beznea break; 61175c88143SClaudiu Beznea } 61275c88143SClaudiu Beznea 61375c88143SClaudiu Beznea /* Second: try to request rate form changeable parent. */ 61475c88143SClaudiu Beznea if (master->chg_pid < 0) 61575c88143SClaudiu Beznea goto end; 61675c88143SClaudiu Beznea 61775c88143SClaudiu Beznea parent = clk_hw_get_parent_by_index(hw, master->chg_pid); 61875c88143SClaudiu Beznea if (!parent) 61975c88143SClaudiu Beznea goto end; 62075c88143SClaudiu Beznea 62175c88143SClaudiu Beznea for (div = 0; div < MASTER_PRES_MAX + 1; div++) { 62275c88143SClaudiu Beznea if (div == MASTER_PRES_MAX) 62375c88143SClaudiu Beznea req_parent.rate = req->rate * 3; 62475c88143SClaudiu Beznea else 62575c88143SClaudiu Beznea req_parent.rate = req->rate << div; 62675c88143SClaudiu Beznea 62775c88143SClaudiu Beznea if (__clk_determine_rate(parent, &req_parent)) 62875c88143SClaudiu Beznea continue; 62975c88143SClaudiu Beznea 63075c88143SClaudiu Beznea clk_sama7g5_master_best_diff(req, parent, req_parent.rate, 63175c88143SClaudiu Beznea &best_rate, &best_diff, div); 63275c88143SClaudiu Beznea 63375c88143SClaudiu Beznea if (!best_diff) 63475c88143SClaudiu Beznea break; 63575c88143SClaudiu Beznea } 63675c88143SClaudiu Beznea 63775c88143SClaudiu Beznea end: 63875c88143SClaudiu Beznea pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n", 63975c88143SClaudiu Beznea __func__, best_rate, 64075c88143SClaudiu Beznea __clk_get_name((req->best_parent_hw)->clk), 64175c88143SClaudiu Beznea req->best_parent_rate); 64275c88143SClaudiu Beznea 64375c88143SClaudiu Beznea if (best_rate < 0) 64475c88143SClaudiu Beznea return -EINVAL; 64575c88143SClaudiu Beznea 64675c88143SClaudiu Beznea req->rate = best_rate; 64775c88143SClaudiu Beznea 64875c88143SClaudiu Beznea return 0; 64975c88143SClaudiu Beznea } 65075c88143SClaudiu Beznea 65175c88143SClaudiu Beznea static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw) 65275c88143SClaudiu Beznea { 65375c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 65475c88143SClaudiu Beznea unsigned long flags; 65575c88143SClaudiu Beznea u8 index; 65675c88143SClaudiu Beznea 65775c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 65875c88143SClaudiu Beznea index = clk_mux_val_to_index(&master->hw, master->mux_table, 0, 65975c88143SClaudiu Beznea master->parent); 66075c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 66175c88143SClaudiu Beznea 66275c88143SClaudiu Beznea return index; 66375c88143SClaudiu Beznea } 66475c88143SClaudiu Beznea 66575c88143SClaudiu Beznea static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index) 66675c88143SClaudiu Beznea { 66775c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 66875c88143SClaudiu Beznea unsigned long flags; 66975c88143SClaudiu Beznea 67075c88143SClaudiu Beznea if (index >= clk_hw_get_num_parents(hw)) 67175c88143SClaudiu Beznea return -EINVAL; 67275c88143SClaudiu Beznea 67375c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 67475c88143SClaudiu Beznea master->parent = clk_mux_index_to_val(master->mux_table, 0, index); 67575c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 67675c88143SClaudiu Beznea 67775c88143SClaudiu Beznea return 0; 67875c88143SClaudiu Beznea } 67975c88143SClaudiu Beznea 68036971566SClaudiu Beznea static void clk_sama7g5_master_set(struct clk_master *master, 68136971566SClaudiu Beznea unsigned int status) 68275c88143SClaudiu Beznea { 68375c88143SClaudiu Beznea unsigned long flags; 68475c88143SClaudiu Beznea unsigned int val, cparent; 685c5538816SClaudiu Beznea unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0; 68688bdeed3SClaudiu Beznea unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT; 68788bdeed3SClaudiu Beznea unsigned int div = master->div << MASTER_DIV_SHIFT; 68875c88143SClaudiu Beznea 68975c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 69075c88143SClaudiu Beznea 691c5538816SClaudiu Beznea regmap_write(master->regmap, AT91_PMC_MCR_V2, 692c5538816SClaudiu Beznea AT91_PMC_MCR_V2_ID(master->id)); 693c5538816SClaudiu Beznea regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); 694c5538816SClaudiu Beznea regmap_update_bits(master->regmap, AT91_PMC_MCR_V2, 695c5538816SClaudiu Beznea enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV | 696c5538816SClaudiu Beznea AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK, 69788bdeed3SClaudiu Beznea enable | parent | div | AT91_PMC_MCR_V2_CMD | 698c5538816SClaudiu Beznea AT91_PMC_MCR_V2_ID(master->id)); 69975c88143SClaudiu Beznea 700c5538816SClaudiu Beznea cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; 70175c88143SClaudiu Beznea 70275c88143SClaudiu Beznea /* Wait here only if parent is being changed. */ 70375c88143SClaudiu Beznea while ((cparent != master->parent) && !clk_master_ready(master)) 70475c88143SClaudiu Beznea cpu_relax(); 70575c88143SClaudiu Beznea 70675c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 70736971566SClaudiu Beznea } 70836971566SClaudiu Beznea 70936971566SClaudiu Beznea static int clk_sama7g5_master_enable(struct clk_hw *hw) 71036971566SClaudiu Beznea { 71136971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 71236971566SClaudiu Beznea 71336971566SClaudiu Beznea clk_sama7g5_master_set(master, 1); 71475c88143SClaudiu Beznea 71575c88143SClaudiu Beznea return 0; 71675c88143SClaudiu Beznea } 71775c88143SClaudiu Beznea 71875c88143SClaudiu Beznea static void clk_sama7g5_master_disable(struct clk_hw *hw) 71975c88143SClaudiu Beznea { 72075c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 72175c88143SClaudiu Beznea unsigned long flags; 72275c88143SClaudiu Beznea 72375c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 72475c88143SClaudiu Beznea 725c5538816SClaudiu Beznea regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); 726c5538816SClaudiu Beznea regmap_update_bits(master->regmap, AT91_PMC_MCR_V2, 727c5538816SClaudiu Beznea AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD | 728c5538816SClaudiu Beznea AT91_PMC_MCR_V2_ID_MSK, 729c5538816SClaudiu Beznea AT91_PMC_MCR_V2_CMD | 730c5538816SClaudiu Beznea AT91_PMC_MCR_V2_ID(master->id)); 73175c88143SClaudiu Beznea 73275c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 73375c88143SClaudiu Beznea } 73475c88143SClaudiu Beznea 73575c88143SClaudiu Beznea static int clk_sama7g5_master_is_enabled(struct clk_hw *hw) 73675c88143SClaudiu Beznea { 73775c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 73875c88143SClaudiu Beznea unsigned long flags; 73975c88143SClaudiu Beznea unsigned int val; 74075c88143SClaudiu Beznea 74175c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 74275c88143SClaudiu Beznea 743c5538816SClaudiu Beznea regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); 744c5538816SClaudiu Beznea regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); 74575c88143SClaudiu Beznea 74675c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 74775c88143SClaudiu Beznea 748c5538816SClaudiu Beznea return !!(val & AT91_PMC_MCR_V2_EN); 74975c88143SClaudiu Beznea } 75075c88143SClaudiu Beznea 75175c88143SClaudiu Beznea static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate, 75275c88143SClaudiu Beznea unsigned long parent_rate) 75375c88143SClaudiu Beznea { 75475c88143SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 75575c88143SClaudiu Beznea unsigned long div, flags; 75675c88143SClaudiu Beznea 75775c88143SClaudiu Beznea div = DIV_ROUND_CLOSEST(parent_rate, rate); 75875c88143SClaudiu Beznea if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1))) 75975c88143SClaudiu Beznea return -EINVAL; 76075c88143SClaudiu Beznea 76175c88143SClaudiu Beznea if (div == 3) 76275c88143SClaudiu Beznea div = MASTER_PRES_MAX; 763c2910c00SClaudiu Beznea else if (div) 76475c88143SClaudiu Beznea div = ffs(div) - 1; 76575c88143SClaudiu Beznea 76675c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 76775c88143SClaudiu Beznea master->div = div; 76875c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 76975c88143SClaudiu Beznea 77075c88143SClaudiu Beznea return 0; 77175c88143SClaudiu Beznea } 77275c88143SClaudiu Beznea 77336971566SClaudiu Beznea static int clk_sama7g5_master_save_context(struct clk_hw *hw) 77436971566SClaudiu Beznea { 77536971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 77636971566SClaudiu Beznea 77736971566SClaudiu Beznea master->pms.status = clk_sama7g5_master_is_enabled(hw); 77836971566SClaudiu Beznea 77936971566SClaudiu Beznea return 0; 78036971566SClaudiu Beznea } 78136971566SClaudiu Beznea 78236971566SClaudiu Beznea static void clk_sama7g5_master_restore_context(struct clk_hw *hw) 78336971566SClaudiu Beznea { 78436971566SClaudiu Beznea struct clk_master *master = to_clk_master(hw); 78536971566SClaudiu Beznea 78636971566SClaudiu Beznea if (master->pms.status) 78736971566SClaudiu Beznea clk_sama7g5_master_set(master, master->pms.status); 78836971566SClaudiu Beznea } 78936971566SClaudiu Beznea 79075c88143SClaudiu Beznea static const struct clk_ops sama7g5_master_ops = { 79175c88143SClaudiu Beznea .enable = clk_sama7g5_master_enable, 79275c88143SClaudiu Beznea .disable = clk_sama7g5_master_disable, 79375c88143SClaudiu Beznea .is_enabled = clk_sama7g5_master_is_enabled, 79475c88143SClaudiu Beznea .recalc_rate = clk_sama7g5_master_recalc_rate, 79575c88143SClaudiu Beznea .determine_rate = clk_sama7g5_master_determine_rate, 79675c88143SClaudiu Beznea .set_rate = clk_sama7g5_master_set_rate, 79775c88143SClaudiu Beznea .get_parent = clk_sama7g5_master_get_parent, 79875c88143SClaudiu Beznea .set_parent = clk_sama7g5_master_set_parent, 79936971566SClaudiu Beznea .save_context = clk_sama7g5_master_save_context, 80036971566SClaudiu Beznea .restore_context = clk_sama7g5_master_restore_context, 80175c88143SClaudiu Beznea }; 80275c88143SClaudiu Beznea 80375c88143SClaudiu Beznea struct clk_hw * __init 80475c88143SClaudiu Beznea at91_clk_sama7g5_register_master(struct regmap *regmap, 80575c88143SClaudiu Beznea const char *name, int num_parents, 80675c88143SClaudiu Beznea const char **parent_names, 80775c88143SClaudiu Beznea u32 *mux_table, 80875c88143SClaudiu Beznea spinlock_t *lock, u8 id, 80975c88143SClaudiu Beznea bool critical, int chg_pid) 81075c88143SClaudiu Beznea { 81175c88143SClaudiu Beznea struct clk_master *master; 81275c88143SClaudiu Beznea struct clk_hw *hw; 81375c88143SClaudiu Beznea struct clk_init_data init; 81475c88143SClaudiu Beznea unsigned long flags; 81575c88143SClaudiu Beznea unsigned int val; 81675c88143SClaudiu Beznea int ret; 81775c88143SClaudiu Beznea 81875c88143SClaudiu Beznea if (!name || !num_parents || !parent_names || !mux_table || 81975c88143SClaudiu Beznea !lock || id > MASTER_MAX_ID) 82075c88143SClaudiu Beznea return ERR_PTR(-EINVAL); 82175c88143SClaudiu Beznea 82275c88143SClaudiu Beznea master = kzalloc(sizeof(*master), GFP_KERNEL); 82375c88143SClaudiu Beznea if (!master) 82475c88143SClaudiu Beznea return ERR_PTR(-ENOMEM); 82575c88143SClaudiu Beznea 82675c88143SClaudiu Beznea init.name = name; 82775c88143SClaudiu Beznea init.ops = &sama7g5_master_ops; 82875c88143SClaudiu Beznea init.parent_names = parent_names; 82975c88143SClaudiu Beznea init.num_parents = num_parents; 83075c88143SClaudiu Beznea init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 83175c88143SClaudiu Beznea if (chg_pid >= 0) 83275c88143SClaudiu Beznea init.flags |= CLK_SET_RATE_PARENT; 83375c88143SClaudiu Beznea if (critical) 83475c88143SClaudiu Beznea init.flags |= CLK_IS_CRITICAL; 83575c88143SClaudiu Beznea 83675c88143SClaudiu Beznea master->hw.init = &init; 83775c88143SClaudiu Beznea master->regmap = regmap; 83875c88143SClaudiu Beznea master->id = id; 83975c88143SClaudiu Beznea master->chg_pid = chg_pid; 84075c88143SClaudiu Beznea master->lock = lock; 84175c88143SClaudiu Beznea master->mux_table = mux_table; 84275c88143SClaudiu Beznea 84375c88143SClaudiu Beznea spin_lock_irqsave(master->lock, flags); 844c5538816SClaudiu Beznea regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); 845c5538816SClaudiu Beznea regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); 846c5538816SClaudiu Beznea master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; 847c5538816SClaudiu Beznea master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT; 84875c88143SClaudiu Beznea spin_unlock_irqrestore(master->lock, flags); 84975c88143SClaudiu Beznea 85075c88143SClaudiu Beznea hw = &master->hw; 85175c88143SClaudiu Beznea ret = clk_hw_register(NULL, &master->hw); 85275c88143SClaudiu Beznea if (ret) { 85375c88143SClaudiu Beznea kfree(master); 85475c88143SClaudiu Beznea hw = ERR_PTR(ret); 85575c88143SClaudiu Beznea } 85675c88143SClaudiu Beznea 85775c88143SClaudiu Beznea return hw; 85875c88143SClaudiu Beznea } 85975c88143SClaudiu Beznea 860b2e39dc0SAlexandre Belloni const struct clk_master_layout at91rm9200_master_layout = { 861e442d234SBoris BREZILLON .mask = 0x31F, 862e442d234SBoris BREZILLON .pres_shift = 2, 863e5be5370SAlexandre Belloni .offset = AT91_PMC_MCKR, 864e442d234SBoris BREZILLON }; 865e442d234SBoris BREZILLON 866b2e39dc0SAlexandre Belloni const struct clk_master_layout at91sam9x5_master_layout = { 867e442d234SBoris BREZILLON .mask = 0x373, 868e442d234SBoris BREZILLON .pres_shift = 4, 869e5be5370SAlexandre Belloni .offset = AT91_PMC_MCKR, 870e442d234SBoris BREZILLON }; 871