Chao Xie | 6b63f02 | 2012-08-20 02:55:11 +0000 | [diff] [blame] | 1 | /* |
| 2 | * mmp factor clock operation source file |
| 3 | * |
| 4 | * Copyright (C) 2012 Marvell |
| 5 | * Chao Xie <xiechao.mail@gmail.com> |
| 6 | * |
| 7 | * This file is licensed under the terms of the GNU General Public |
| 8 | * License version 2. This program is licensed "as is" without any |
| 9 | * warranty of any kind, whether express or implied. |
| 10 | */ |
| 11 | |
| 12 | #include <linux/clk-provider.h> |
| 13 | #include <linux/slab.h> |
| 14 | #include <linux/io.h> |
| 15 | #include <linux/err.h> |
| 16 | |
| 17 | #include "clk.h" |
| 18 | /* |
| 19 | * It is M/N clock |
| 20 | * |
| 21 | * Fout from synthesizer can be given from two equations: |
| 22 | * numerator/denominator = Fin / (Fout * factor) |
| 23 | */ |
| 24 | |
| 25 | #define to_clk_factor(hw) container_of(hw, struct clk_factor, hw) |
| 26 | struct clk_factor { |
| 27 | struct clk_hw hw; |
| 28 | void __iomem *base; |
| 29 | struct clk_factor_masks *masks; |
| 30 | struct clk_factor_tbl *ftbl; |
| 31 | unsigned int ftbl_cnt; |
| 32 | }; |
| 33 | |
| 34 | static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate, |
| 35 | unsigned long *prate) |
| 36 | { |
| 37 | struct clk_factor *factor = to_clk_factor(hw); |
| 38 | unsigned long rate = 0, prev_rate; |
| 39 | int i; |
| 40 | |
| 41 | for (i = 0; i < factor->ftbl_cnt; i++) { |
| 42 | prev_rate = rate; |
| 43 | rate = (((*prate / 10000) * factor->ftbl[i].num) / |
| 44 | (factor->ftbl[i].den * factor->masks->factor)) * 10000; |
| 45 | if (rate > drate) |
| 46 | break; |
| 47 | } |
| 48 | if (i == 0) |
| 49 | return rate; |
| 50 | else |
| 51 | return prev_rate; |
| 52 | } |
| 53 | |
| 54 | static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, |
| 55 | unsigned long parent_rate) |
| 56 | { |
| 57 | struct clk_factor *factor = to_clk_factor(hw); |
| 58 | struct clk_factor_masks *masks = factor->masks; |
| 59 | unsigned int val, num, den; |
| 60 | |
| 61 | val = readl_relaxed(factor->base); |
| 62 | |
| 63 | /* calculate numerator */ |
| 64 | num = (val >> masks->num_shift) & masks->num_mask; |
| 65 | |
| 66 | /* calculate denominator */ |
Chao Xie | 7433ab4 | 2014-01-23 10:47:40 +0800 | [diff] [blame^] | 67 | den = (val >> masks->den_shift) & masks->den_mask; |
Chao Xie | 6b63f02 | 2012-08-20 02:55:11 +0000 | [diff] [blame] | 68 | |
| 69 | if (!den) |
| 70 | return 0; |
| 71 | |
| 72 | return (((parent_rate / 10000) * den) / |
| 73 | (num * factor->masks->factor)) * 10000; |
| 74 | } |
| 75 | |
| 76 | /* Configures new clock rate*/ |
| 77 | static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, |
| 78 | unsigned long prate) |
| 79 | { |
| 80 | struct clk_factor *factor = to_clk_factor(hw); |
| 81 | struct clk_factor_masks *masks = factor->masks; |
| 82 | int i; |
| 83 | unsigned long val; |
| 84 | unsigned long prev_rate, rate = 0; |
| 85 | |
| 86 | for (i = 0; i < factor->ftbl_cnt; i++) { |
| 87 | prev_rate = rate; |
| 88 | rate = (((prate / 10000) * factor->ftbl[i].num) / |
| 89 | (factor->ftbl[i].den * factor->masks->factor)) * 10000; |
| 90 | if (rate > drate) |
| 91 | break; |
| 92 | } |
| 93 | if (i > 0) |
| 94 | i--; |
| 95 | |
| 96 | val = readl_relaxed(factor->base); |
| 97 | |
| 98 | val &= ~(masks->num_mask << masks->num_shift); |
| 99 | val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; |
| 100 | |
| 101 | val &= ~(masks->den_mask << masks->den_shift); |
| 102 | val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; |
| 103 | |
| 104 | writel_relaxed(val, factor->base); |
| 105 | |
| 106 | return 0; |
| 107 | } |
| 108 | |
| 109 | static struct clk_ops clk_factor_ops = { |
| 110 | .recalc_rate = clk_factor_recalc_rate, |
| 111 | .round_rate = clk_factor_round_rate, |
| 112 | .set_rate = clk_factor_set_rate, |
| 113 | }; |
| 114 | |
| 115 | struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, |
| 116 | unsigned long flags, void __iomem *base, |
| 117 | struct clk_factor_masks *masks, struct clk_factor_tbl *ftbl, |
| 118 | unsigned int ftbl_cnt) |
| 119 | { |
| 120 | struct clk_factor *factor; |
| 121 | struct clk_init_data init; |
| 122 | struct clk *clk; |
| 123 | |
| 124 | if (!masks) { |
| 125 | pr_err("%s: must pass a clk_factor_mask\n", __func__); |
| 126 | return ERR_PTR(-EINVAL); |
| 127 | } |
| 128 | |
| 129 | factor = kzalloc(sizeof(*factor), GFP_KERNEL); |
| 130 | if (!factor) { |
| 131 | pr_err("%s: could not allocate factor clk\n", __func__); |
| 132 | return ERR_PTR(-ENOMEM); |
| 133 | } |
| 134 | |
| 135 | /* struct clk_aux assignments */ |
| 136 | factor->base = base; |
| 137 | factor->masks = masks; |
| 138 | factor->ftbl = ftbl; |
| 139 | factor->ftbl_cnt = ftbl_cnt; |
| 140 | factor->hw.init = &init; |
| 141 | |
| 142 | init.name = name; |
| 143 | init.ops = &clk_factor_ops; |
| 144 | init.flags = flags; |
| 145 | init.parent_names = &parent_name; |
| 146 | init.num_parents = 1; |
| 147 | |
| 148 | clk = clk_register(NULL, &factor->hw); |
| 149 | if (IS_ERR_OR_NULL(clk)) |
| 150 | kfree(factor); |
| 151 | |
| 152 | return clk; |
| 153 | } |