1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/clkdev.h> |
8 | #include <linux/clk/at91_pmc.h> |
9 | #include <linux/of.h> |
10 | #include <linux/mfd/syscon.h> |
11 | #include <linux/regmap.h> |
12 | |
13 | #include "pmc.h" |
14 | |
15 | #define PROG_ID_MAX 7 |
16 | |
17 | #define PROG_STATUS_MASK(id) (1 << ((id) + 8)) |
18 | #define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask) |
19 | #define PROG_MAX_RM9200_CSS 3 |
20 | |
21 | struct clk_programmable { |
22 | struct clk_hw hw; |
23 | struct regmap *regmap; |
24 | u32 *mux_table; |
25 | u8 id; |
26 | const struct clk_programmable_layout *layout; |
27 | struct at91_clk_pms pms; |
28 | }; |
29 | |
30 | #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) |
31 | |
32 | static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, |
33 | unsigned long parent_rate) |
34 | { |
35 | struct clk_programmable *prog = to_clk_programmable(hw); |
36 | const struct clk_programmable_layout *layout = prog->layout; |
37 | unsigned int pckr; |
38 | unsigned long rate; |
39 | |
40 | regmap_read(map: prog->regmap, AT91_PMC_PCKR(prog->id), val: &pckr); |
41 | |
42 | if (layout->is_pres_direct) |
43 | rate = parent_rate / (PROG_PRES(layout, pckr) + 1); |
44 | else |
45 | rate = parent_rate >> PROG_PRES(layout, pckr); |
46 | |
47 | return rate; |
48 | } |
49 | |
50 | static int clk_programmable_determine_rate(struct clk_hw *hw, |
51 | struct clk_rate_request *req) |
52 | { |
53 | struct clk_programmable *prog = to_clk_programmable(hw); |
54 | const struct clk_programmable_layout *layout = prog->layout; |
55 | struct clk_hw *parent; |
56 | long best_rate = -EINVAL; |
57 | unsigned long parent_rate; |
58 | unsigned long tmp_rate = 0; |
59 | int shift; |
60 | int i; |
61 | |
62 | for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
63 | parent = clk_hw_get_parent_by_index(hw, index: i); |
64 | if (!parent) |
65 | continue; |
66 | |
67 | parent_rate = clk_hw_get_rate(hw: parent); |
68 | if (layout->is_pres_direct) { |
69 | for (shift = 0; shift <= layout->pres_mask; shift++) { |
70 | tmp_rate = parent_rate / (shift + 1); |
71 | if (tmp_rate <= req->rate) |
72 | break; |
73 | } |
74 | } else { |
75 | for (shift = 0; shift < layout->pres_mask; shift++) { |
76 | tmp_rate = parent_rate >> shift; |
77 | if (tmp_rate <= req->rate) |
78 | break; |
79 | } |
80 | } |
81 | |
82 | if (tmp_rate > req->rate) |
83 | continue; |
84 | |
85 | if (best_rate < 0 || |
86 | (req->rate - tmp_rate) < (req->rate - best_rate)) { |
87 | best_rate = tmp_rate; |
88 | req->best_parent_rate = parent_rate; |
89 | req->best_parent_hw = parent; |
90 | } |
91 | |
92 | if (!best_rate) |
93 | break; |
94 | } |
95 | |
96 | if (best_rate < 0) |
97 | return best_rate; |
98 | |
99 | req->rate = best_rate; |
100 | return 0; |
101 | } |
102 | |
103 | static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) |
104 | { |
105 | struct clk_programmable *prog = to_clk_programmable(hw); |
106 | const struct clk_programmable_layout *layout = prog->layout; |
107 | unsigned int mask = layout->css_mask; |
108 | unsigned int pckr = index; |
109 | |
110 | if (layout->have_slck_mck) |
111 | mask |= AT91_PMC_CSSMCK_MCK; |
112 | |
113 | if (prog->mux_table) |
114 | pckr = clk_mux_index_to_val(table: prog->mux_table, flags: 0, index); |
115 | |
116 | if (index > layout->css_mask) { |
117 | if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck) |
118 | return -EINVAL; |
119 | |
120 | pckr |= AT91_PMC_CSSMCK_MCK; |
121 | } |
122 | |
123 | regmap_update_bits(map: prog->regmap, AT91_PMC_PCKR(prog->id), mask, val: pckr); |
124 | |
125 | return 0; |
126 | } |
127 | |
128 | static u8 clk_programmable_get_parent(struct clk_hw *hw) |
129 | { |
130 | struct clk_programmable *prog = to_clk_programmable(hw); |
131 | const struct clk_programmable_layout *layout = prog->layout; |
132 | unsigned int pckr; |
133 | u8 ret; |
134 | |
135 | regmap_read(map: prog->regmap, AT91_PMC_PCKR(prog->id), val: &pckr); |
136 | |
137 | ret = pckr & layout->css_mask; |
138 | |
139 | if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret) |
140 | ret = PROG_MAX_RM9200_CSS + 1; |
141 | |
142 | if (prog->mux_table) |
143 | ret = clk_mux_val_to_index(hw: &prog->hw, table: prog->mux_table, flags: 0, val: ret); |
144 | |
145 | return ret; |
146 | } |
147 | |
148 | static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, |
149 | unsigned long parent_rate) |
150 | { |
151 | struct clk_programmable *prog = to_clk_programmable(hw); |
152 | const struct clk_programmable_layout *layout = prog->layout; |
153 | unsigned long div = parent_rate / rate; |
154 | int shift = 0; |
155 | |
156 | if (!div) |
157 | return -EINVAL; |
158 | |
159 | if (layout->is_pres_direct) { |
160 | shift = div - 1; |
161 | |
162 | if (shift > layout->pres_mask) |
163 | return -EINVAL; |
164 | } else { |
165 | shift = fls(x: div) - 1; |
166 | |
167 | if (div != (1 << shift)) |
168 | return -EINVAL; |
169 | |
170 | if (shift >= layout->pres_mask) |
171 | return -EINVAL; |
172 | } |
173 | |
174 | regmap_update_bits(map: prog->regmap, AT91_PMC_PCKR(prog->id), |
175 | mask: layout->pres_mask << layout->pres_shift, |
176 | val: shift << layout->pres_shift); |
177 | |
178 | return 0; |
179 | } |
180 | |
181 | static int clk_programmable_save_context(struct clk_hw *hw) |
182 | { |
183 | struct clk_programmable *prog = to_clk_programmable(hw); |
184 | struct clk_hw *parent_hw = clk_hw_get_parent(hw); |
185 | |
186 | prog->pms.parent = clk_programmable_get_parent(hw); |
187 | prog->pms.parent_rate = clk_hw_get_rate(hw: parent_hw); |
188 | prog->pms.rate = clk_programmable_recalc_rate(hw, parent_rate: prog->pms.parent_rate); |
189 | |
190 | return 0; |
191 | } |
192 | |
193 | static void clk_programmable_restore_context(struct clk_hw *hw) |
194 | { |
195 | struct clk_programmable *prog = to_clk_programmable(hw); |
196 | int ret; |
197 | |
198 | ret = clk_programmable_set_parent(hw, index: prog->pms.parent); |
199 | if (ret) |
200 | return; |
201 | |
202 | clk_programmable_set_rate(hw, rate: prog->pms.rate, parent_rate: prog->pms.parent_rate); |
203 | } |
204 | |
205 | static const struct clk_ops programmable_ops = { |
206 | .recalc_rate = clk_programmable_recalc_rate, |
207 | .determine_rate = clk_programmable_determine_rate, |
208 | .get_parent = clk_programmable_get_parent, |
209 | .set_parent = clk_programmable_set_parent, |
210 | .set_rate = clk_programmable_set_rate, |
211 | .save_context = clk_programmable_save_context, |
212 | .restore_context = clk_programmable_restore_context, |
213 | }; |
214 | |
215 | struct clk_hw * __init |
216 | at91_clk_register_programmable(struct regmap *regmap, |
217 | const char *name, const char **parent_names, |
218 | struct clk_hw **parent_hws, u8 num_parents, u8 id, |
219 | const struct clk_programmable_layout *layout, |
220 | u32 *mux_table) |
221 | { |
222 | struct clk_programmable *prog; |
223 | struct clk_hw *hw; |
224 | struct clk_init_data init = {}; |
225 | int ret; |
226 | |
227 | if (id > PROG_ID_MAX || !(parent_names || parent_hws)) |
228 | return ERR_PTR(error: -EINVAL); |
229 | |
230 | prog = kzalloc(size: sizeof(*prog), GFP_KERNEL); |
231 | if (!prog) |
232 | return ERR_PTR(error: -ENOMEM); |
233 | |
234 | init.name = name; |
235 | init.ops = &programmable_ops; |
236 | if (parent_hws) |
237 | init.parent_hws = (const struct clk_hw **)parent_hws; |
238 | else |
239 | init.parent_names = parent_names; |
240 | init.num_parents = num_parents; |
241 | init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; |
242 | |
243 | prog->id = id; |
244 | prog->layout = layout; |
245 | prog->hw.init = &init; |
246 | prog->regmap = regmap; |
247 | prog->mux_table = mux_table; |
248 | |
249 | hw = &prog->hw; |
250 | ret = clk_hw_register(NULL, hw: &prog->hw); |
251 | if (ret) { |
252 | kfree(objp: prog); |
253 | hw = ERR_PTR(error: ret); |
254 | } |
255 | |
256 | return hw; |
257 | } |
258 | |
259 | const struct clk_programmable_layout at91rm9200_programmable_layout = { |
260 | .pres_mask = 0x7, |
261 | .pres_shift = 2, |
262 | .css_mask = 0x3, |
263 | .have_slck_mck = 0, |
264 | .is_pres_direct = 0, |
265 | }; |
266 | |
267 | const struct clk_programmable_layout at91sam9g45_programmable_layout = { |
268 | .pres_mask = 0x7, |
269 | .pres_shift = 2, |
270 | .css_mask = 0x3, |
271 | .have_slck_mck = 1, |
272 | .is_pres_direct = 0, |
273 | }; |
274 | |
275 | const struct clk_programmable_layout at91sam9x5_programmable_layout = { |
276 | .pres_mask = 0x7, |
277 | .pres_shift = 4, |
278 | .css_mask = 0x7, |
279 | .have_slck_mck = 0, |
280 | .is_pres_direct = 0, |
281 | }; |
282 | |