Commit | Line | Data |
---|---|---|
6b63f023 CX |
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 | ||
2bd1e256 | 25 | #define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw) |
6b63f023 CX |
26 | |
27 | static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate, | |
28 | unsigned long *prate) | |
29 | { | |
2bd1e256 | 30 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
6b63f023 CX |
31 | unsigned long rate = 0, prev_rate; |
32 | int i; | |
33 | ||
34 | for (i = 0; i < factor->ftbl_cnt; i++) { | |
35 | prev_rate = rate; | |
c45693a6 CX |
36 | rate = (((*prate / 10000) * factor->ftbl[i].den) / |
37 | (factor->ftbl[i].num * factor->masks->factor)) * 10000; | |
6b63f023 CX |
38 | if (rate > drate) |
39 | break; | |
40 | } | |
5d26c15d | 41 | if ((i == 0) || (i == factor->ftbl_cnt)) { |
6b63f023 | 42 | return rate; |
5d26c15d CX |
43 | } else { |
44 | if ((drate - prev_rate) > (rate - drate)) | |
45 | return rate; | |
46 | else | |
47 | return prev_rate; | |
48 | } | |
6b63f023 CX |
49 | } |
50 | ||
51 | static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, | |
52 | unsigned long parent_rate) | |
53 | { | |
2bd1e256 CX |
54 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
55 | struct mmp_clk_factor_masks *masks = factor->masks; | |
6b63f023 CX |
56 | unsigned int val, num, den; |
57 | ||
58 | val = readl_relaxed(factor->base); | |
59 | ||
60 | /* calculate numerator */ | |
61 | num = (val >> masks->num_shift) & masks->num_mask; | |
62 | ||
63 | /* calculate denominator */ | |
7433ab43 | 64 | den = (val >> masks->den_shift) & masks->den_mask; |
6b63f023 CX |
65 | |
66 | if (!den) | |
67 | return 0; | |
68 | ||
69 | return (((parent_rate / 10000) * den) / | |
70 | (num * factor->masks->factor)) * 10000; | |
71 | } | |
72 | ||
73 | /* Configures new clock rate*/ | |
74 | static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, | |
75 | unsigned long prate) | |
76 | { | |
2bd1e256 CX |
77 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
78 | struct mmp_clk_factor_masks *masks = factor->masks; | |
6b63f023 CX |
79 | int i; |
80 | unsigned long val; | |
81 | unsigned long prev_rate, rate = 0; | |
61256133 | 82 | unsigned long flags = 0; |
6b63f023 CX |
83 | |
84 | for (i = 0; i < factor->ftbl_cnt; i++) { | |
85 | prev_rate = rate; | |
c45693a6 CX |
86 | rate = (((prate / 10000) * factor->ftbl[i].den) / |
87 | (factor->ftbl[i].num * factor->masks->factor)) * 10000; | |
6b63f023 CX |
88 | if (rate > drate) |
89 | break; | |
90 | } | |
91 | if (i > 0) | |
92 | i--; | |
93 | ||
61256133 CX |
94 | if (factor->lock) |
95 | spin_lock_irqsave(factor->lock, flags); | |
96 | ||
6b63f023 CX |
97 | val = readl_relaxed(factor->base); |
98 | ||
99 | val &= ~(masks->num_mask << masks->num_shift); | |
100 | val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; | |
101 | ||
102 | val &= ~(masks->den_mask << masks->den_shift); | |
103 | val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; | |
104 | ||
105 | writel_relaxed(val, factor->base); | |
106 | ||
61256133 CX |
107 | if (factor->lock) |
108 | spin_unlock_irqrestore(factor->lock, flags); | |
109 | ||
6b63f023 CX |
110 | return 0; |
111 | } | |
112 | ||
74fc23aa | 113 | static void clk_factor_init(struct clk_hw *hw) |
0c4c11f3 CX |
114 | { |
115 | struct mmp_clk_factor *factor = to_clk_factor(hw); | |
116 | struct mmp_clk_factor_masks *masks = factor->masks; | |
117 | u32 val, num, den; | |
118 | int i; | |
119 | unsigned long flags = 0; | |
120 | ||
121 | if (factor->lock) | |
122 | spin_lock_irqsave(factor->lock, flags); | |
123 | ||
124 | val = readl(factor->base); | |
125 | ||
126 | /* calculate numerator */ | |
127 | num = (val >> masks->num_shift) & masks->num_mask; | |
128 | ||
129 | /* calculate denominator */ | |
130 | den = (val >> masks->den_shift) & masks->den_mask; | |
131 | ||
132 | for (i = 0; i < factor->ftbl_cnt; i++) | |
133 | if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) | |
134 | break; | |
135 | ||
136 | if (i >= factor->ftbl_cnt) { | |
137 | val &= ~(masks->num_mask << masks->num_shift); | |
138 | val |= (factor->ftbl[0].num & masks->num_mask) << | |
139 | masks->num_shift; | |
140 | ||
141 | val &= ~(masks->den_mask << masks->den_shift); | |
142 | val |= (factor->ftbl[0].den & masks->den_mask) << | |
143 | masks->den_shift; | |
144 | ||
145 | writel(val, factor->base); | |
146 | } | |
147 | ||
148 | if (factor->lock) | |
149 | spin_unlock_irqrestore(factor->lock, flags); | |
150 | } | |
151 | ||
6b63f023 CX |
152 | static struct clk_ops clk_factor_ops = { |
153 | .recalc_rate = clk_factor_recalc_rate, | |
154 | .round_rate = clk_factor_round_rate, | |
155 | .set_rate = clk_factor_set_rate, | |
0c4c11f3 | 156 | .init = clk_factor_init, |
6b63f023 CX |
157 | }; |
158 | ||
159 | struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, | |
160 | unsigned long flags, void __iomem *base, | |
2bd1e256 CX |
161 | struct mmp_clk_factor_masks *masks, |
162 | struct mmp_clk_factor_tbl *ftbl, | |
61256133 | 163 | unsigned int ftbl_cnt, spinlock_t *lock) |
6b63f023 | 164 | { |
2bd1e256 | 165 | struct mmp_clk_factor *factor; |
6b63f023 CX |
166 | struct clk_init_data init; |
167 | struct clk *clk; | |
168 | ||
169 | if (!masks) { | |
170 | pr_err("%s: must pass a clk_factor_mask\n", __func__); | |
171 | return ERR_PTR(-EINVAL); | |
172 | } | |
173 | ||
174 | factor = kzalloc(sizeof(*factor), GFP_KERNEL); | |
175 | if (!factor) { | |
176 | pr_err("%s: could not allocate factor clk\n", __func__); | |
177 | return ERR_PTR(-ENOMEM); | |
178 | } | |
179 | ||
180 | /* struct clk_aux assignments */ | |
181 | factor->base = base; | |
182 | factor->masks = masks; | |
183 | factor->ftbl = ftbl; | |
184 | factor->ftbl_cnt = ftbl_cnt; | |
185 | factor->hw.init = &init; | |
61256133 | 186 | factor->lock = lock; |
6b63f023 CX |
187 | |
188 | init.name = name; | |
189 | init.ops = &clk_factor_ops; | |
190 | init.flags = flags; | |
191 | init.parent_names = &parent_name; | |
192 | init.num_parents = 1; | |
193 | ||
194 | clk = clk_register(NULL, &factor->hw); | |
195 | if (IS_ERR_OR_NULL(clk)) | |
196 | kfree(factor); | |
197 | ||
198 | return clk; | |
199 | } |