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
clk_programmable_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(prog->regmap, AT91_PMC_PCKR(prog->id), &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
clk_programmable_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)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, i);
64 if (!parent)
65 continue;
66
67 parent_rate = clk_hw_get_rate(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
clk_programmable_set_parent(struct clk_hw * hw,u8 index)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(prog->mux_table, 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(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr);
124
125 return 0;
126 }
127
clk_programmable_get_parent(struct clk_hw * hw)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(prog->regmap, AT91_PMC_PCKR(prog->id), &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(&prog->hw, prog->mux_table, 0, ret);
144
145 return ret;
146 }
147
clk_programmable_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)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(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(prog->regmap, AT91_PMC_PCKR(prog->id),
175 layout->pres_mask << layout->pres_shift,
176 shift << layout->pres_shift);
177
178 return 0;
179 }
180
clk_programmable_save_context(struct clk_hw * hw)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(parent_hw);
188 prog->pms.rate = clk_programmable_recalc_rate(hw, prog->pms.parent_rate);
189
190 return 0;
191 }
192
clk_programmable_restore_context(struct clk_hw * hw)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, prog->pms.parent);
199 if (ret)
200 return;
201
202 clk_programmable_set_rate(hw, prog->pms.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
at91_clk_register_programmable(struct regmap * regmap,const char * name,const char ** parent_names,struct clk_hw ** parent_hws,u8 num_parents,u8 id,const struct clk_programmable_layout * layout,u32 * mux_table)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(-EINVAL);
229
230 prog = kzalloc(sizeof(*prog), GFP_KERNEL);
231 if (!prog)
232 return ERR_PTR(-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, &prog->hw);
251 if (ret) {
252 kfree(prog);
253 hw = ERR_PTR(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