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(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 
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 
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 
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 
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 
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 
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
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