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 SAM9X5_USB_DIV_SHIFT	8
16 #define SAM9X5_USB_MAX_DIV	0xf
17 
18 #define RM9200_USB_DIV_SHIFT	28
19 #define RM9200_USB_DIV_TAB_SIZE	4
20 
21 #define SAM9X5_USBS_MASK	GENMASK(0, 0)
22 #define SAM9X60_USBS_MASK	GENMASK(1, 0)
23 
24 struct at91sam9x5_clk_usb {
25 	struct clk_hw hw;
26 	struct regmap *regmap;
27 	struct at91_clk_pms pms;
28 	u32 usbs_mask;
29 	u8 num_parents;
30 };
31 
32 #define to_at91sam9x5_clk_usb(hw) \
33 	container_of(hw, struct at91sam9x5_clk_usb, hw)
34 
35 struct at91rm9200_clk_usb {
36 	struct clk_hw hw;
37 	struct regmap *regmap;
38 	u32 divisors[4];
39 };
40 
41 #define to_at91rm9200_clk_usb(hw) \
42 	container_of(hw, struct at91rm9200_clk_usb, hw)
43 
at91sam9x5_clk_usb_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)44 static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw,
45 						    unsigned long parent_rate)
46 {
47 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
48 	unsigned int usbr;
49 	u8 usbdiv;
50 
51 	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
52 	usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT;
53 
54 	return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1));
55 }
56 
at91sam9x5_clk_usb_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)57 static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw,
58 					     struct clk_rate_request *req)
59 {
60 	struct clk_hw *parent;
61 	long best_rate = -EINVAL;
62 	unsigned long tmp_rate;
63 	int best_diff = -1;
64 	int tmp_diff;
65 	int i;
66 
67 	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
68 		int div;
69 
70 		parent = clk_hw_get_parent_by_index(hw, i);
71 		if (!parent)
72 			continue;
73 
74 		for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) {
75 			unsigned long tmp_parent_rate;
76 
77 			tmp_parent_rate = req->rate * div;
78 			tmp_parent_rate = clk_hw_round_rate(parent,
79 							   tmp_parent_rate);
80 			if (!tmp_parent_rate)
81 				continue;
82 
83 			tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div);
84 			if (tmp_rate < req->rate)
85 				tmp_diff = req->rate - tmp_rate;
86 			else
87 				tmp_diff = tmp_rate - req->rate;
88 
89 			if (best_diff < 0 || best_diff > tmp_diff) {
90 				best_rate = tmp_rate;
91 				best_diff = tmp_diff;
92 				req->best_parent_rate = tmp_parent_rate;
93 				req->best_parent_hw = parent;
94 			}
95 
96 			if (!best_diff || tmp_rate < req->rate)
97 				break;
98 		}
99 
100 		if (!best_diff)
101 			break;
102 	}
103 
104 	if (best_rate < 0)
105 		return best_rate;
106 
107 	req->rate = best_rate;
108 	return 0;
109 }
110 
at91sam9x5_clk_usb_set_parent(struct clk_hw * hw,u8 index)111 static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index)
112 {
113 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
114 
115 	if (index >= usb->num_parents)
116 		return -EINVAL;
117 
118 	regmap_update_bits(usb->regmap, AT91_PMC_USB, usb->usbs_mask, index);
119 
120 	return 0;
121 }
122 
at91sam9x5_clk_usb_get_parent(struct clk_hw * hw)123 static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw)
124 {
125 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
126 	unsigned int usbr;
127 
128 	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
129 
130 	return usbr & usb->usbs_mask;
131 }
132 
at91sam9x5_clk_usb_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)133 static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
134 				       unsigned long parent_rate)
135 {
136 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
137 	unsigned long div;
138 
139 	if (!rate)
140 		return -EINVAL;
141 
142 	div = DIV_ROUND_CLOSEST(parent_rate, rate);
143 	if (div > SAM9X5_USB_MAX_DIV + 1 || !div)
144 		return -EINVAL;
145 
146 	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV,
147 			   (div - 1) << SAM9X5_USB_DIV_SHIFT);
148 
149 	return 0;
150 }
151 
at91sam9x5_usb_save_context(struct clk_hw * hw)152 static int at91sam9x5_usb_save_context(struct clk_hw *hw)
153 {
154 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
155 	struct clk_hw *parent_hw = clk_hw_get_parent(hw);
156 
157 	usb->pms.parent = at91sam9x5_clk_usb_get_parent(hw);
158 	usb->pms.parent_rate = clk_hw_get_rate(parent_hw);
159 	usb->pms.rate = at91sam9x5_clk_usb_recalc_rate(hw, usb->pms.parent_rate);
160 
161 	return 0;
162 }
163 
at91sam9x5_usb_restore_context(struct clk_hw * hw)164 static void at91sam9x5_usb_restore_context(struct clk_hw *hw)
165 {
166 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
167 	int ret;
168 
169 	ret = at91sam9x5_clk_usb_set_parent(hw, usb->pms.parent);
170 	if (ret)
171 		return;
172 
173 	at91sam9x5_clk_usb_set_rate(hw, usb->pms.rate, usb->pms.parent_rate);
174 }
175 
176 static const struct clk_ops at91sam9x5_usb_ops = {
177 	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
178 	.determine_rate = at91sam9x5_clk_usb_determine_rate,
179 	.get_parent = at91sam9x5_clk_usb_get_parent,
180 	.set_parent = at91sam9x5_clk_usb_set_parent,
181 	.set_rate = at91sam9x5_clk_usb_set_rate,
182 	.save_context = at91sam9x5_usb_save_context,
183 	.restore_context = at91sam9x5_usb_restore_context,
184 };
185 
at91sam9n12_clk_usb_enable(struct clk_hw * hw)186 static int at91sam9n12_clk_usb_enable(struct clk_hw *hw)
187 {
188 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
189 
190 	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS,
191 			   AT91_PMC_USBS);
192 
193 	return 0;
194 }
195 
at91sam9n12_clk_usb_disable(struct clk_hw * hw)196 static void at91sam9n12_clk_usb_disable(struct clk_hw *hw)
197 {
198 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
199 
200 	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 0);
201 }
202 
at91sam9n12_clk_usb_is_enabled(struct clk_hw * hw)203 static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw)
204 {
205 	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
206 	unsigned int usbr;
207 
208 	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
209 
210 	return usbr & AT91_PMC_USBS;
211 }
212 
213 static const struct clk_ops at91sam9n12_usb_ops = {
214 	.enable = at91sam9n12_clk_usb_enable,
215 	.disable = at91sam9n12_clk_usb_disable,
216 	.is_enabled = at91sam9n12_clk_usb_is_enabled,
217 	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
218 	.determine_rate = at91sam9x5_clk_usb_determine_rate,
219 	.set_rate = at91sam9x5_clk_usb_set_rate,
220 };
221 
222 static struct clk_hw * __init
_at91sam9x5_clk_register_usb(struct regmap * regmap,const char * name,const char ** parent_names,u8 num_parents,u32 usbs_mask)223 _at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
224 			     const char **parent_names, u8 num_parents,
225 			     u32 usbs_mask)
226 {
227 	struct at91sam9x5_clk_usb *usb;
228 	struct clk_hw *hw;
229 	struct clk_init_data init;
230 	int ret;
231 
232 	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
233 	if (!usb)
234 		return ERR_PTR(-ENOMEM);
235 
236 	init.name = name;
237 	init.ops = &at91sam9x5_usb_ops;
238 	init.parent_names = parent_names;
239 	init.num_parents = num_parents;
240 	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
241 		     CLK_SET_RATE_PARENT;
242 
243 	usb->hw.init = &init;
244 	usb->regmap = regmap;
245 	usb->usbs_mask = usbs_mask;
246 	usb->num_parents = num_parents;
247 
248 	hw = &usb->hw;
249 	ret = clk_hw_register(NULL, &usb->hw);
250 	if (ret) {
251 		kfree(usb);
252 		hw = ERR_PTR(ret);
253 	}
254 
255 	return hw;
256 }
257 
258 struct clk_hw * __init
at91sam9x5_clk_register_usb(struct regmap * regmap,const char * name,const char ** parent_names,u8 num_parents)259 at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
260 			    const char **parent_names, u8 num_parents)
261 {
262 	return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
263 					    num_parents, SAM9X5_USBS_MASK);
264 }
265 
266 struct clk_hw * __init
sam9x60_clk_register_usb(struct regmap * regmap,const char * name,const char ** parent_names,u8 num_parents)267 sam9x60_clk_register_usb(struct regmap *regmap, const char *name,
268 			 const char **parent_names, u8 num_parents)
269 {
270 	return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
271 					    num_parents, SAM9X60_USBS_MASK);
272 }
273 
274 struct clk_hw * __init
at91sam9n12_clk_register_usb(struct regmap * regmap,const char * name,const char * parent_name)275 at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name,
276 			     const char *parent_name)
277 {
278 	struct at91sam9x5_clk_usb *usb;
279 	struct clk_hw *hw;
280 	struct clk_init_data init;
281 	int ret;
282 
283 	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
284 	if (!usb)
285 		return ERR_PTR(-ENOMEM);
286 
287 	init.name = name;
288 	init.ops = &at91sam9n12_usb_ops;
289 	init.parent_names = &parent_name;
290 	init.num_parents = 1;
291 	init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT;
292 
293 	usb->hw.init = &init;
294 	usb->regmap = regmap;
295 
296 	hw = &usb->hw;
297 	ret = clk_hw_register(NULL, &usb->hw);
298 	if (ret) {
299 		kfree(usb);
300 		hw = ERR_PTR(ret);
301 	}
302 
303 	return hw;
304 }
305 
at91rm9200_clk_usb_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)306 static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw,
307 						    unsigned long parent_rate)
308 {
309 	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
310 	unsigned int pllbr;
311 	u8 usbdiv;
312 
313 	regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr);
314 
315 	usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT;
316 	if (usb->divisors[usbdiv])
317 		return parent_rate / usb->divisors[usbdiv];
318 
319 	return 0;
320 }
321 
at91rm9200_clk_usb_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)322 static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,
323 					  unsigned long *parent_rate)
324 {
325 	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
326 	struct clk_hw *parent = clk_hw_get_parent(hw);
327 	unsigned long bestrate = 0;
328 	int bestdiff = -1;
329 	unsigned long tmprate;
330 	int tmpdiff;
331 	int i = 0;
332 
333 	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
334 		unsigned long tmp_parent_rate;
335 
336 		if (!usb->divisors[i])
337 			continue;
338 
339 		tmp_parent_rate = rate * usb->divisors[i];
340 		tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate);
341 		tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]);
342 		if (tmprate < rate)
343 			tmpdiff = rate - tmprate;
344 		else
345 			tmpdiff = tmprate - rate;
346 
347 		if (bestdiff < 0 || bestdiff > tmpdiff) {
348 			bestrate = tmprate;
349 			bestdiff = tmpdiff;
350 			*parent_rate = tmp_parent_rate;
351 		}
352 
353 		if (!bestdiff)
354 			break;
355 	}
356 
357 	return bestrate;
358 }
359 
at91rm9200_clk_usb_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)360 static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
361 				       unsigned long parent_rate)
362 {
363 	int i;
364 	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
365 	unsigned long div;
366 
367 	if (!rate)
368 		return -EINVAL;
369 
370 	div = DIV_ROUND_CLOSEST(parent_rate, rate);
371 
372 	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
373 		if (usb->divisors[i] == div) {
374 			regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR,
375 					   AT91_PMC_USBDIV,
376 					   i << RM9200_USB_DIV_SHIFT);
377 
378 			return 0;
379 		}
380 	}
381 
382 	return -EINVAL;
383 }
384 
385 static const struct clk_ops at91rm9200_usb_ops = {
386 	.recalc_rate = at91rm9200_clk_usb_recalc_rate,
387 	.round_rate = at91rm9200_clk_usb_round_rate,
388 	.set_rate = at91rm9200_clk_usb_set_rate,
389 };
390 
391 struct clk_hw * __init
at91rm9200_clk_register_usb(struct regmap * regmap,const char * name,const char * parent_name,const u32 * divisors)392 at91rm9200_clk_register_usb(struct regmap *regmap, const char *name,
393 			    const char *parent_name, const u32 *divisors)
394 {
395 	struct at91rm9200_clk_usb *usb;
396 	struct clk_hw *hw;
397 	struct clk_init_data init;
398 	int ret;
399 
400 	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
401 	if (!usb)
402 		return ERR_PTR(-ENOMEM);
403 
404 	init.name = name;
405 	init.ops = &at91rm9200_usb_ops;
406 	init.parent_names = &parent_name;
407 	init.num_parents = 1;
408 	init.flags = CLK_SET_RATE_PARENT;
409 
410 	usb->hw.init = &init;
411 	usb->regmap = regmap;
412 	memcpy(usb->divisors, divisors, sizeof(usb->divisors));
413 
414 	hw = &usb->hw;
415 	ret = clk_hw_register(NULL, &usb->hw);
416 	if (ret) {
417 		kfree(usb);
418 		hw = ERR_PTR(ret);
419 	}
420 
421 	return hw;
422 }
423