clk-programmable.c revision cce6db80a049b23f867f4afded70ca8027876a08 
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
29struct clk_programmable_layout {
30	u8 pres_shift;
31	u8 css_mask;
32	u8 have_slck_mck;
33};
34
35struct 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
44static 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
57static long clk_programmable_determine_rate(struct clk_hw *hw,
58					    unsigned long rate,
59					    unsigned long *best_parent_rate,
60					    struct clk **best_parent_clk)
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_clk = parent;
88		}
89
90		if (!best_rate)
91			break;
92	}
93
94	return best_rate;
95}
96
97static 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
120static 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
136static 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 best_rate = parent_rate;
143	unsigned long best_diff;
144	unsigned long new_diff;
145	unsigned long cur_rate;
146	int shift = 0;
147	u32 tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)) &
148		  ~(PROG_PRES_MASK << layout->pres_shift);
149
150	if (rate > parent_rate)
151		return parent_rate;
152	else
153		best_diff = parent_rate - rate;
154
155	if (!best_diff) {
156		pmc_write(pmc, AT91_PMC_PCKR(prog->id), tmp | shift);
157		return 0;
158	}
159
160	for (shift = 1; shift < PROG_PRES_MASK; shift++) {
161		cur_rate = parent_rate >> shift;
162
163		if (cur_rate > rate)
164			new_diff = cur_rate - rate;
165		else
166			new_diff = rate - cur_rate;
167
168		if (!new_diff)
169			break;
170
171		if (new_diff < best_diff) {
172			best_diff = new_diff;
173			best_rate = cur_rate;
174		}
175
176		if (rate > cur_rate)
177			break;
178	}
179
180	pmc_write(pmc, AT91_PMC_PCKR(prog->id),
181		  tmp | (shift << layout->pres_shift));
182
183	return 0;
184}
185
186static const struct clk_ops programmable_ops = {
187	.recalc_rate = clk_programmable_recalc_rate,
188	.determine_rate = clk_programmable_determine_rate,
189	.get_parent = clk_programmable_get_parent,
190	.set_parent = clk_programmable_set_parent,
191	.set_rate = clk_programmable_set_rate,
192};
193
194static struct clk * __init
195at91_clk_register_programmable(struct at91_pmc *pmc,
196			       const char *name, const char **parent_names,
197			       u8 num_parents, u8 id,
198			       const struct clk_programmable_layout *layout)
199{
200	struct clk_programmable *prog;
201	struct clk *clk = NULL;
202	struct clk_init_data init;
203
204	if (id > PROG_ID_MAX)
205		return ERR_PTR(-EINVAL);
206
207	prog = kzalloc(sizeof(*prog), GFP_KERNEL);
208	if (!prog)
209		return ERR_PTR(-ENOMEM);
210
211	init.name = name;
212	init.ops = &programmable_ops;
213	init.parent_names = parent_names;
214	init.num_parents = num_parents;
215	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
216
217	prog->id = id;
218	prog->layout = layout;
219	prog->hw.init = &init;
220	prog->pmc = pmc;
221
222	clk = clk_register(NULL, &prog->hw);
223	if (IS_ERR(clk))
224		kfree(prog);
225
226	return clk;
227}
228
229static const struct clk_programmable_layout at91rm9200_programmable_layout = {
230	.pres_shift = 2,
231	.css_mask = 0x3,
232	.have_slck_mck = 0,
233};
234
235static const struct clk_programmable_layout at91sam9g45_programmable_layout = {
236	.pres_shift = 2,
237	.css_mask = 0x3,
238	.have_slck_mck = 1,
239};
240
241static const struct clk_programmable_layout at91sam9x5_programmable_layout = {
242	.pres_shift = 4,
243	.css_mask = 0x7,
244	.have_slck_mck = 0,
245};
246
247static void __init
248of_at91_clk_prog_setup(struct device_node *np, struct at91_pmc *pmc,
249		       const struct clk_programmable_layout *layout)
250{
251	int num;
252	u32 id;
253	int i;
254	struct clk *clk;
255	int num_parents;
256	const char *parent_names[PROG_SOURCE_MAX];
257	const char *name;
258	struct device_node *progclknp;
259
260	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
261	if (num_parents <= 0 || num_parents > PROG_SOURCE_MAX)
262		return;
263
264	for (i = 0; i < num_parents; ++i) {
265		parent_names[i] = of_clk_get_parent_name(np, i);
266		if (!parent_names[i])
267			return;
268	}
269
270	num = of_get_child_count(np);
271	if (!num || num > (PROG_ID_MAX + 1))
272		return;
273
274	for_each_child_of_node(np, progclknp) {
275		if (of_property_read_u32(progclknp, "reg", &id))
276			continue;
277
278		if (of_property_read_string(np, "clock-output-names", &name))
279			name = progclknp->name;
280
281		clk = at91_clk_register_programmable(pmc, name,
282						     parent_names, num_parents,
283						     id, layout);
284		if (IS_ERR(clk))
285			continue;
286
287		of_clk_add_provider(progclknp, of_clk_src_simple_get, clk);
288	}
289}
290
291
292void __init of_at91rm9200_clk_prog_setup(struct device_node *np,
293					 struct at91_pmc *pmc)
294{
295	of_at91_clk_prog_setup(np, pmc, &at91rm9200_programmable_layout);
296}
297
298void __init of_at91sam9g45_clk_prog_setup(struct device_node *np,
299					  struct at91_pmc *pmc)
300{
301	of_at91_clk_prog_setup(np, pmc, &at91sam9g45_programmable_layout);
302}
303
304void __init of_at91sam9x5_clk_prog_setup(struct device_node *np,
305					 struct at91_pmc *pmc)
306{
307	of_at91_clk_prog_setup(np, pmc, &at91sam9x5_programmable_layout);
308}
309