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