16b63f023SChao Xie /* 26b63f023SChao Xie * mmp factor clock operation source file 36b63f023SChao Xie * 46b63f023SChao Xie * Copyright (C) 2012 Marvell 56b63f023SChao Xie * Chao Xie <xiechao.mail@gmail.com> 66b63f023SChao Xie * 76b63f023SChao Xie * This file is licensed under the terms of the GNU General Public 86b63f023SChao Xie * License version 2. This program is licensed "as is" without any 96b63f023SChao Xie * warranty of any kind, whether express or implied. 106b63f023SChao Xie */ 116b63f023SChao Xie 126b63f023SChao Xie #include <linux/clk-provider.h> 136b63f023SChao Xie #include <linux/slab.h> 146b63f023SChao Xie #include <linux/io.h> 156b63f023SChao Xie #include <linux/err.h> 166b63f023SChao Xie 176b63f023SChao Xie #include "clk.h" 186b63f023SChao Xie /* 196b63f023SChao Xie * It is M/N clock 206b63f023SChao Xie * 216b63f023SChao Xie * Fout from synthesizer can be given from two equations: 226b63f023SChao Xie * numerator/denominator = Fin / (Fout * factor) 236b63f023SChao Xie */ 246b63f023SChao Xie 252bd1e256SChao Xie #define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw) 266b63f023SChao Xie 276b63f023SChao Xie static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate, 286b63f023SChao Xie unsigned long *prate) 296b63f023SChao Xie { 302bd1e256SChao Xie struct mmp_clk_factor *factor = to_clk_factor(hw); 3106030c4eSLubomir Rintel u64 rate = 0, prev_rate; 326b63f023SChao Xie int i; 336b63f023SChao Xie 346b63f023SChao Xie for (i = 0; i < factor->ftbl_cnt; i++) { 356b63f023SChao Xie prev_rate = rate; 3606030c4eSLubomir Rintel rate = *prate; 3706030c4eSLubomir Rintel rate *= factor->ftbl[i].den; 3806030c4eSLubomir Rintel do_div(rate, factor->ftbl[i].num * factor->masks->factor); 3906030c4eSLubomir Rintel 406b63f023SChao Xie if (rate > drate) 416b63f023SChao Xie break; 426b63f023SChao Xie } 435d26c15dSChao Xie if ((i == 0) || (i == factor->ftbl_cnt)) { 445d26c15dSChao Xie return rate; 455d26c15dSChao Xie } else { 465d26c15dSChao Xie if ((drate - prev_rate) > (rate - drate)) 476b63f023SChao Xie return rate; 486b63f023SChao Xie else 496b63f023SChao Xie return prev_rate; 506b63f023SChao Xie } 515d26c15dSChao Xie } 526b63f023SChao Xie 536b63f023SChao Xie static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, 546b63f023SChao Xie unsigned long parent_rate) 556b63f023SChao Xie { 562bd1e256SChao Xie struct mmp_clk_factor *factor = to_clk_factor(hw); 572bd1e256SChao Xie struct mmp_clk_factor_masks *masks = factor->masks; 586b63f023SChao Xie unsigned int val, num, den; 5906030c4eSLubomir Rintel u64 rate; 606b63f023SChao Xie 616b63f023SChao Xie val = readl_relaxed(factor->base); 626b63f023SChao Xie 636b63f023SChao Xie /* calculate numerator */ 646b63f023SChao Xie num = (val >> masks->num_shift) & masks->num_mask; 656b63f023SChao Xie 666b63f023SChao Xie /* calculate denominator */ 677433ab43SChao Xie den = (val >> masks->den_shift) & masks->den_mask; 686b63f023SChao Xie 696b63f023SChao Xie if (!den) 706b63f023SChao Xie return 0; 716b63f023SChao Xie 7206030c4eSLubomir Rintel rate = parent_rate; 7306030c4eSLubomir Rintel rate *= den; 7406030c4eSLubomir Rintel do_div(rate, num * factor->masks->factor); 7506030c4eSLubomir Rintel 7606030c4eSLubomir Rintel return rate; 776b63f023SChao Xie } 786b63f023SChao Xie 796b63f023SChao Xie /* Configures new clock rate*/ 806b63f023SChao Xie static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, 816b63f023SChao Xie unsigned long prate) 826b63f023SChao Xie { 832bd1e256SChao Xie struct mmp_clk_factor *factor = to_clk_factor(hw); 842bd1e256SChao Xie struct mmp_clk_factor_masks *masks = factor->masks; 856b63f023SChao Xie int i; 866b63f023SChao Xie unsigned long val; 8761256133SChao Xie unsigned long flags = 0; 8806030c4eSLubomir Rintel u64 rate = 0; 896b63f023SChao Xie 906b63f023SChao Xie for (i = 0; i < factor->ftbl_cnt; i++) { 9106030c4eSLubomir Rintel rate = prate; 9206030c4eSLubomir Rintel rate *= factor->ftbl[i].den; 9306030c4eSLubomir Rintel do_div(rate, factor->ftbl[i].num * factor->masks->factor); 9406030c4eSLubomir Rintel 956b63f023SChao Xie if (rate > drate) 966b63f023SChao Xie break; 976b63f023SChao Xie } 986b63f023SChao Xie if (i > 0) 996b63f023SChao Xie i--; 1006b63f023SChao Xie 10161256133SChao Xie if (factor->lock) 10261256133SChao Xie spin_lock_irqsave(factor->lock, flags); 10361256133SChao Xie 1046b63f023SChao Xie val = readl_relaxed(factor->base); 1056b63f023SChao Xie 1066b63f023SChao Xie val &= ~(masks->num_mask << masks->num_shift); 1076b63f023SChao Xie val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; 1086b63f023SChao Xie 1096b63f023SChao Xie val &= ~(masks->den_mask << masks->den_shift); 1106b63f023SChao Xie val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; 1116b63f023SChao Xie 1126b63f023SChao Xie writel_relaxed(val, factor->base); 1136b63f023SChao Xie 11461256133SChao Xie if (factor->lock) 11561256133SChao Xie spin_unlock_irqrestore(factor->lock, flags); 11661256133SChao Xie 1176b63f023SChao Xie return 0; 1186b63f023SChao Xie } 1196b63f023SChao Xie 12089d079dcSJerome Brunet static int clk_factor_init(struct clk_hw *hw) 1210c4c11f3SChao Xie { 1220c4c11f3SChao Xie struct mmp_clk_factor *factor = to_clk_factor(hw); 1230c4c11f3SChao Xie struct mmp_clk_factor_masks *masks = factor->masks; 1240c4c11f3SChao Xie u32 val, num, den; 1250c4c11f3SChao Xie int i; 1260c4c11f3SChao Xie unsigned long flags = 0; 1270c4c11f3SChao Xie 1280c4c11f3SChao Xie if (factor->lock) 1290c4c11f3SChao Xie spin_lock_irqsave(factor->lock, flags); 1300c4c11f3SChao Xie 1310c4c11f3SChao Xie val = readl(factor->base); 1320c4c11f3SChao Xie 1330c4c11f3SChao Xie /* calculate numerator */ 1340c4c11f3SChao Xie num = (val >> masks->num_shift) & masks->num_mask; 1350c4c11f3SChao Xie 1360c4c11f3SChao Xie /* calculate denominator */ 1370c4c11f3SChao Xie den = (val >> masks->den_shift) & masks->den_mask; 1380c4c11f3SChao Xie 1390c4c11f3SChao Xie for (i = 0; i < factor->ftbl_cnt; i++) 1400c4c11f3SChao Xie if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) 1410c4c11f3SChao Xie break; 1420c4c11f3SChao Xie 1430c4c11f3SChao Xie if (i >= factor->ftbl_cnt) { 1440c4c11f3SChao Xie val &= ~(masks->num_mask << masks->num_shift); 1450c4c11f3SChao Xie val |= (factor->ftbl[0].num & masks->num_mask) << 1460c4c11f3SChao Xie masks->num_shift; 1470c4c11f3SChao Xie 1480c4c11f3SChao Xie val &= ~(masks->den_mask << masks->den_shift); 1490c4c11f3SChao Xie val |= (factor->ftbl[0].den & masks->den_mask) << 1500c4c11f3SChao Xie masks->den_shift; 151*5278acc4SLubomir Rintel } 1520c4c11f3SChao Xie 153*5278acc4SLubomir Rintel if (!(val & masks->enable_mask) || i >= factor->ftbl_cnt) { 154*5278acc4SLubomir Rintel val |= masks->enable_mask; 1550c4c11f3SChao Xie writel(val, factor->base); 1560c4c11f3SChao Xie } 1570c4c11f3SChao Xie 1580c4c11f3SChao Xie if (factor->lock) 1590c4c11f3SChao Xie spin_unlock_irqrestore(factor->lock, flags); 16089d079dcSJerome Brunet 16189d079dcSJerome Brunet return 0; 1620c4c11f3SChao Xie } 1630c4c11f3SChao Xie 164e90a7da4SBhumika Goyal static const struct clk_ops clk_factor_ops = { 1656b63f023SChao Xie .recalc_rate = clk_factor_recalc_rate, 1666b63f023SChao Xie .round_rate = clk_factor_round_rate, 1676b63f023SChao Xie .set_rate = clk_factor_set_rate, 1680c4c11f3SChao Xie .init = clk_factor_init, 1696b63f023SChao Xie }; 1706b63f023SChao Xie 1716b63f023SChao Xie struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, 1726b63f023SChao Xie unsigned long flags, void __iomem *base, 1732bd1e256SChao Xie struct mmp_clk_factor_masks *masks, 1742bd1e256SChao Xie struct mmp_clk_factor_tbl *ftbl, 17561256133SChao Xie unsigned int ftbl_cnt, spinlock_t *lock) 1766b63f023SChao Xie { 1772bd1e256SChao Xie struct mmp_clk_factor *factor; 1786b63f023SChao Xie struct clk_init_data init; 1796b63f023SChao Xie struct clk *clk; 1806b63f023SChao Xie 1816b63f023SChao Xie if (!masks) { 1826b63f023SChao Xie pr_err("%s: must pass a clk_factor_mask\n", __func__); 1836b63f023SChao Xie return ERR_PTR(-EINVAL); 1846b63f023SChao Xie } 1856b63f023SChao Xie 1866b63f023SChao Xie factor = kzalloc(sizeof(*factor), GFP_KERNEL); 1871cc36f73SMarkus Elfring if (!factor) 1886b63f023SChao Xie return ERR_PTR(-ENOMEM); 1896b63f023SChao Xie 1906b63f023SChao Xie /* struct clk_aux assignments */ 1916b63f023SChao Xie factor->base = base; 1926b63f023SChao Xie factor->masks = masks; 1936b63f023SChao Xie factor->ftbl = ftbl; 1946b63f023SChao Xie factor->ftbl_cnt = ftbl_cnt; 1956b63f023SChao Xie factor->hw.init = &init; 19661256133SChao Xie factor->lock = lock; 1976b63f023SChao Xie 1986b63f023SChao Xie init.name = name; 1996b63f023SChao Xie init.ops = &clk_factor_ops; 2006b63f023SChao Xie init.flags = flags; 2016b63f023SChao Xie init.parent_names = &parent_name; 2026b63f023SChao Xie init.num_parents = 1; 2036b63f023SChao Xie 2046b63f023SChao Xie clk = clk_register(NULL, &factor->hw); 2056b63f023SChao Xie if (IS_ERR_OR_NULL(clk)) 2066b63f023SChao Xie kfree(factor); 2076b63f023SChao Xie 2086b63f023SChao Xie return clk; 2096b63f023SChao Xie } 210