1 /*
2  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
3  *
4  * This program is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; either version 2 of the License, or
7  * (at your option) any later version.
8  *
9  */
10 
11 #include <linux/clk-provider.h>
12 #include <linux/clkdev.h>
13 #include <linux/clk/at91_pmc.h>
14 #include <linux/of.h>
15 #include <linux/of_address.h>
16 #include <linux/io.h>
17 #include <linux/wait.h>
18 #include <linux/sched.h>
19 
20 #include "pmc.h"
21 
22 #define PROG_SOURCE_MAX		5
23 #define PROG_ID_MAX		7
24 
25 #define PROG_STATUS_MASK(id)	(1 << ((id) + 8))
26 #define PROG_PRES_MASK		0x7
27 #define PROG_MAX_RM9200_CSS	3
28 
29 struct clk_programmable_layout {
30 	u8 pres_shift;
31 	u8 css_mask;
32 	u8 have_slck_mck;
33 };
34 
35 struct clk_programmable {
36 	struct clk_hw hw;
37 	struct at91_pmc *pmc;
38 	u8 id;
39 	const struct clk_programmable_layout *layout;
40 };
41 
42 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)
43 
44 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,
45 						  unsigned long parent_rate)
46 {
47 	u32 pres;
48 	struct clk_programmable *prog = to_clk_programmable(hw);
49 	struct at91_pmc *pmc = prog->pmc;
50 	const struct clk_programmable_layout *layout = prog->layout;
51 
52 	pres = (pmc_read(pmc, AT91_PMC_PCKR(prog->id)) >> layout->pres_shift) &
53 	       PROG_PRES_MASK;
54 	return parent_rate >> pres;
55 }
56 
57 static long clk_programmable_determine_rate(struct clk_hw *hw,
58 					    unsigned long rate,
59 					    unsigned long *best_parent_rate,
60 					    struct clk_hw **best_parent_hw)
61 {
62 	struct clk *parent = NULL;
63 	long best_rate = -EINVAL;
64 	unsigned long parent_rate;
65 	unsigned long tmp_rate;
66 	int shift;
67 	int i;
68 
69 	for (i = 0; i < __clk_get_num_parents(hw->clk); i++) {
70 		parent = clk_get_parent_by_index(hw->clk, i);
71 		if (!parent)
72 			continue;
73 
74 		parent_rate = __clk_get_rate(parent);
75 		for (shift = 0; shift < PROG_PRES_MASK; shift++) {
76 			tmp_rate = parent_rate >> shift;
77 			if (tmp_rate <= rate)
78 				break;
79 		}
80 
81 		if (tmp_rate > rate)
82 			continue;
83 
84 		if (best_rate < 0 || (rate - tmp_rate) < (rate - best_rate)) {
85 			best_rate = tmp_rate;
86 			*best_parent_rate = parent_rate;
87 			*best_parent_hw = __clk_get_hw(parent);
88 		}
89 
90 		if (!best_rate)
91 			break;
92 	}
93 
94 	return best_rate;
95 }
96 
97 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index)
98 {
99 	struct clk_programmable *prog = to_clk_programmable(hw);
100 	const struct clk_programmable_layout *layout = prog->layout;
101 	struct at91_pmc *pmc = prog->pmc;
102 	u32 tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)) & ~layout->css_mask;
103 
104 	if (layout->have_slck_mck)
105 		tmp &= AT91_PMC_CSSMCK_MCK;
106 
107 	if (index > layout->css_mask) {
108 		if (index > PROG_MAX_RM9200_CSS && layout->have_slck_mck) {
109 			tmp |= AT91_PMC_CSSMCK_MCK;
110 			return 0;
111 		} else {
112 			return -EINVAL;
113 		}
114 	}
115 
116 	pmc_write(pmc, AT91_PMC_PCKR(prog->id), tmp | index);
117 	return 0;
118 }
119 
120 static u8 clk_programmable_get_parent(struct clk_hw *hw)
121 {
122 	u32 tmp;
123 	u8 ret;
124 	struct clk_programmable *prog = to_clk_programmable(hw);
125 	struct at91_pmc *pmc = prog->pmc;
126 	const struct clk_programmable_layout *layout = prog->layout;
127 
128 	tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id));
129 	ret = tmp & layout->css_mask;
130 	if (layout->have_slck_mck && (tmp & AT91_PMC_CSSMCK_MCK) && !ret)
131 		ret = PROG_MAX_RM9200_CSS + 1;
132 
133 	return ret;
134 }
135 
136 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
137 				     unsigned long parent_rate)
138 {
139 	struct clk_programmable *prog = to_clk_programmable(hw);
140 	struct at91_pmc *pmc = prog->pmc;
141 	const struct clk_programmable_layout *layout = prog->layout;
142 	unsigned long div = parent_rate / rate;
143 	int shift = 0;
144 	u32 tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)) &
145 		  ~(PROG_PRES_MASK << layout->pres_shift);
146 
147 	if (!div)
148 		return -EINVAL;
149 
150 	shift = fls(div) - 1;
151 
152 	if (div != (1<<shift))
153 		return -EINVAL;
154 
155 	if (shift >= PROG_PRES_MASK)
156 		return -EINVAL;
157 
158 	pmc_write(pmc, AT91_PMC_PCKR(prog->id),
159 		  tmp | (shift << layout->pres_shift));
160 
161 	return 0;
162 }
163 
164 static const struct clk_ops programmable_ops = {
165 	.recalc_rate = clk_programmable_recalc_rate,
166 	.determine_rate = clk_programmable_determine_rate,
167 	.get_parent = clk_programmable_get_parent,
168 	.set_parent = clk_programmable_set_parent,
169 	.set_rate = clk_programmable_set_rate,
170 };
171 
172 static struct clk * __init
173 at91_clk_register_programmable(struct at91_pmc *pmc,
174 			       const char *name, const char **parent_names,
175 			       u8 num_parents, u8 id,
176 			       const struct clk_programmable_layout *layout)
177 {
178 	struct clk_programmable *prog;
179 	struct clk *clk = NULL;
180 	struct clk_init_data init;
181 
182 	if (id > PROG_ID_MAX)
183 		return ERR_PTR(-EINVAL);
184 
185 	prog = kzalloc(sizeof(*prog), GFP_KERNEL);
186 	if (!prog)
187 		return ERR_PTR(-ENOMEM);
188 
189 	init.name = name;
190 	init.ops = &programmable_ops;
191 	init.parent_names = parent_names;
192 	init.num_parents = num_parents;
193 	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
194 
195 	prog->id = id;
196 	prog->layout = layout;
197 	prog->hw.init = &init;
198 	prog->pmc = pmc;
199 
200 	clk = clk_register(NULL, &prog->hw);
201 	if (IS_ERR(clk))
202 		kfree(prog);
203 
204 	return clk;
205 }
206 
207 static const struct clk_programmable_layout at91rm9200_programmable_layout = {
208 	.pres_shift = 2,
209 	.css_mask = 0x3,
210 	.have_slck_mck = 0,
211 };
212 
213 static const struct clk_programmable_layout at91sam9g45_programmable_layout = {
214 	.pres_shift = 2,
215 	.css_mask = 0x3,
216 	.have_slck_mck = 1,
217 };
218 
219 static const struct clk_programmable_layout at91sam9x5_programmable_layout = {
220 	.pres_shift = 4,
221 	.css_mask = 0x7,
222 	.have_slck_mck = 0,
223 };
224 
225 static void __init
226 of_at91_clk_prog_setup(struct device_node *np, struct at91_pmc *pmc,
227 		       const struct clk_programmable_layout *layout)
228 {
229 	int num;
230 	u32 id;
231 	int i;
232 	struct clk *clk;
233 	int num_parents;
234 	const char *parent_names[PROG_SOURCE_MAX];
235 	const char *name;
236 	struct device_node *progclknp;
237 
238 	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
239 	if (num_parents <= 0 || num_parents > PROG_SOURCE_MAX)
240 		return;
241 
242 	for (i = 0; i < num_parents; ++i) {
243 		parent_names[i] = of_clk_get_parent_name(np, i);
244 		if (!parent_names[i])
245 			return;
246 	}
247 
248 	num = of_get_child_count(np);
249 	if (!num || num > (PROG_ID_MAX + 1))
250 		return;
251 
252 	for_each_child_of_node(np, progclknp) {
253 		if (of_property_read_u32(progclknp, "reg", &id))
254 			continue;
255 
256 		if (of_property_read_string(np, "clock-output-names", &name))
257 			name = progclknp->name;
258 
259 		clk = at91_clk_register_programmable(pmc, name,
260 						     parent_names, num_parents,
261 						     id, layout);
262 		if (IS_ERR(clk))
263 			continue;
264 
265 		of_clk_add_provider(progclknp, of_clk_src_simple_get, clk);
266 	}
267 }
268 
269 
270 void __init of_at91rm9200_clk_prog_setup(struct device_node *np,
271 					 struct at91_pmc *pmc)
272 {
273 	of_at91_clk_prog_setup(np, pmc, &at91rm9200_programmable_layout);
274 }
275 
276 void __init of_at91sam9g45_clk_prog_setup(struct device_node *np,
277 					  struct at91_pmc *pmc)
278 {
279 	of_at91_clk_prog_setup(np, pmc, &at91sam9g45_programmable_layout);
280 }
281 
282 void __init of_at91sam9x5_clk_prog_setup(struct device_node *np,
283 					 struct at91_pmc *pmc)
284 {
285 	of_at91_clk_prog_setup(np, pmc, &at91sam9x5_programmable_layout);
286 }
287