1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (C) 2016 Maxime Ripard |
4 | * Maxime Ripard <maxime.ripard@free-electrons.com> |
5 | */ |
6 | |
7 | #include <linux/clk-provider.h> |
8 | #include <linux/io.h> |
9 | |
10 | #include "ccu_gate.h" |
11 | #include "ccu_nkm.h" |
12 | |
13 | struct _ccu_nkm { |
14 | unsigned long n, min_n, max_n; |
15 | unsigned long k, min_k, max_k; |
16 | unsigned long m, min_m, max_m; |
17 | }; |
18 | |
19 | static unsigned long ccu_nkm_find_best_with_parent_adj(struct ccu_common *common, |
20 | struct clk_hw *parent_hw, |
21 | unsigned long *parent, unsigned long rate, |
22 | struct _ccu_nkm *nkm) |
23 | { |
24 | unsigned long best_rate = 0, best_parent_rate = *parent, tmp_parent = *parent; |
25 | unsigned long best_n = 0, best_k = 0, best_m = 0; |
26 | unsigned long _n, _k, _m; |
27 | |
28 | for (_k = nkm->min_k; _k <= nkm->max_k; _k++) { |
29 | for (_n = nkm->min_n; _n <= nkm->max_n; _n++) { |
30 | for (_m = nkm->min_m; _m <= nkm->max_m; _m++) { |
31 | unsigned long tmp_rate; |
32 | |
33 | tmp_parent = clk_hw_round_rate(hw: parent_hw, rate: rate * _m / (_n * _k)); |
34 | |
35 | tmp_rate = tmp_parent * _n * _k / _m; |
36 | |
37 | if (ccu_is_better_rate(common, target_rate: rate, current_rate: tmp_rate, best_rate) || |
38 | (tmp_parent == *parent && tmp_rate == best_rate)) { |
39 | best_rate = tmp_rate; |
40 | best_parent_rate = tmp_parent; |
41 | best_n = _n; |
42 | best_k = _k; |
43 | best_m = _m; |
44 | } |
45 | } |
46 | } |
47 | } |
48 | |
49 | nkm->n = best_n; |
50 | nkm->k = best_k; |
51 | nkm->m = best_m; |
52 | |
53 | *parent = best_parent_rate; |
54 | |
55 | return best_rate; |
56 | } |
57 | |
58 | static unsigned long ccu_nkm_find_best(unsigned long parent, unsigned long rate, |
59 | struct _ccu_nkm *nkm, struct ccu_common *common) |
60 | { |
61 | unsigned long best_rate = 0; |
62 | unsigned long best_n = 0, best_k = 0, best_m = 0; |
63 | unsigned long _n, _k, _m; |
64 | |
65 | for (_k = nkm->min_k; _k <= nkm->max_k; _k++) { |
66 | for (_n = nkm->min_n; _n <= nkm->max_n; _n++) { |
67 | for (_m = nkm->min_m; _m <= nkm->max_m; _m++) { |
68 | unsigned long tmp_rate; |
69 | |
70 | tmp_rate = parent * _n * _k / _m; |
71 | |
72 | if (ccu_is_better_rate(common, target_rate: rate, current_rate: tmp_rate, best_rate)) { |
73 | best_rate = tmp_rate; |
74 | best_n = _n; |
75 | best_k = _k; |
76 | best_m = _m; |
77 | } |
78 | } |
79 | } |
80 | } |
81 | |
82 | nkm->n = best_n; |
83 | nkm->k = best_k; |
84 | nkm->m = best_m; |
85 | |
86 | return best_rate; |
87 | } |
88 | |
89 | static void ccu_nkm_disable(struct clk_hw *hw) |
90 | { |
91 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
92 | |
93 | return ccu_gate_helper_disable(common: &nkm->common, gate: nkm->enable); |
94 | } |
95 | |
96 | static int ccu_nkm_enable(struct clk_hw *hw) |
97 | { |
98 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
99 | |
100 | return ccu_gate_helper_enable(common: &nkm->common, gate: nkm->enable); |
101 | } |
102 | |
103 | static int ccu_nkm_is_enabled(struct clk_hw *hw) |
104 | { |
105 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
106 | |
107 | return ccu_gate_helper_is_enabled(common: &nkm->common, gate: nkm->enable); |
108 | } |
109 | |
110 | static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw, |
111 | unsigned long parent_rate) |
112 | { |
113 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
114 | unsigned long n, m, k, rate; |
115 | u32 reg; |
116 | |
117 | reg = readl(addr: nkm->common.base + nkm->common.reg); |
118 | |
119 | n = reg >> nkm->n.shift; |
120 | n &= (1 << nkm->n.width) - 1; |
121 | n += nkm->n.offset; |
122 | if (!n) |
123 | n++; |
124 | |
125 | k = reg >> nkm->k.shift; |
126 | k &= (1 << nkm->k.width) - 1; |
127 | k += nkm->k.offset; |
128 | if (!k) |
129 | k++; |
130 | |
131 | m = reg >> nkm->m.shift; |
132 | m &= (1 << nkm->m.width) - 1; |
133 | m += nkm->m.offset; |
134 | if (!m) |
135 | m++; |
136 | |
137 | rate = parent_rate * n * k / m; |
138 | |
139 | if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV) |
140 | rate /= nkm->fixed_post_div; |
141 | |
142 | return rate; |
143 | } |
144 | |
145 | static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux, |
146 | struct clk_hw *parent_hw, |
147 | unsigned long *parent_rate, |
148 | unsigned long rate, |
149 | void *data) |
150 | { |
151 | struct ccu_nkm *nkm = data; |
152 | struct _ccu_nkm _nkm; |
153 | |
154 | _nkm.min_n = nkm->n.min ?: 1; |
155 | _nkm.max_n = nkm->n.max ?: 1 << nkm->n.width; |
156 | _nkm.min_k = nkm->k.min ?: 1; |
157 | _nkm.max_k = nkm->k.max ?: 1 << nkm->k.width; |
158 | _nkm.min_m = 1; |
159 | _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width; |
160 | |
161 | if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV) |
162 | rate *= nkm->fixed_post_div; |
163 | |
164 | if (!clk_hw_can_set_rate_parent(&nkm->common.hw)) |
165 | rate = ccu_nkm_find_best(parent: *parent_rate, rate, nkm: &_nkm, common: &nkm->common); |
166 | else |
167 | rate = ccu_nkm_find_best_with_parent_adj(common: &nkm->common, parent_hw, parent: parent_rate, rate, |
168 | nkm: &_nkm); |
169 | |
170 | if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV) |
171 | rate /= nkm->fixed_post_div; |
172 | |
173 | return rate; |
174 | } |
175 | |
176 | static int ccu_nkm_determine_rate(struct clk_hw *hw, |
177 | struct clk_rate_request *req) |
178 | { |
179 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
180 | |
181 | return ccu_mux_helper_determine_rate(common: &nkm->common, cm: &nkm->mux, |
182 | req, round: ccu_nkm_round_rate, data: nkm); |
183 | } |
184 | |
185 | static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate, |
186 | unsigned long parent_rate) |
187 | { |
188 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
189 | struct _ccu_nkm _nkm; |
190 | unsigned long flags; |
191 | u32 reg; |
192 | |
193 | if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV) |
194 | rate *= nkm->fixed_post_div; |
195 | |
196 | _nkm.min_n = nkm->n.min ?: 1; |
197 | _nkm.max_n = nkm->n.max ?: 1 << nkm->n.width; |
198 | _nkm.min_k = nkm->k.min ?: 1; |
199 | _nkm.max_k = nkm->k.max ?: 1 << nkm->k.width; |
200 | _nkm.min_m = 1; |
201 | _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width; |
202 | |
203 | ccu_nkm_find_best(parent: parent_rate, rate, nkm: &_nkm, common: &nkm->common); |
204 | |
205 | spin_lock_irqsave(nkm->common.lock, flags); |
206 | |
207 | reg = readl(addr: nkm->common.base + nkm->common.reg); |
208 | reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift); |
209 | reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift); |
210 | reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift); |
211 | |
212 | reg |= (_nkm.n - nkm->n.offset) << nkm->n.shift; |
213 | reg |= (_nkm.k - nkm->k.offset) << nkm->k.shift; |
214 | reg |= (_nkm.m - nkm->m.offset) << nkm->m.shift; |
215 | writel(val: reg, addr: nkm->common.base + nkm->common.reg); |
216 | |
217 | spin_unlock_irqrestore(lock: nkm->common.lock, flags); |
218 | |
219 | ccu_helper_wait_for_lock(common: &nkm->common, lock: nkm->lock); |
220 | |
221 | return 0; |
222 | } |
223 | |
224 | static u8 ccu_nkm_get_parent(struct clk_hw *hw) |
225 | { |
226 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
227 | |
228 | return ccu_mux_helper_get_parent(common: &nkm->common, cm: &nkm->mux); |
229 | } |
230 | |
231 | static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index) |
232 | { |
233 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); |
234 | |
235 | return ccu_mux_helper_set_parent(common: &nkm->common, cm: &nkm->mux, index); |
236 | } |
237 | |
238 | const struct clk_ops ccu_nkm_ops = { |
239 | .disable = ccu_nkm_disable, |
240 | .enable = ccu_nkm_enable, |
241 | .is_enabled = ccu_nkm_is_enabled, |
242 | |
243 | .get_parent = ccu_nkm_get_parent, |
244 | .set_parent = ccu_nkm_set_parent, |
245 | |
246 | .determine_rate = ccu_nkm_determine_rate, |
247 | .recalc_rate = ccu_nkm_recalc_rate, |
248 | .set_rate = ccu_nkm_set_rate, |
249 | }; |
250 | EXPORT_SYMBOL_NS_GPL(ccu_nkm_ops, SUNXI_CCU); |
251 | |