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/mfd/syscon.h>
16 #include <linux/regmap.h>
17 
18 #include "pmc.h"
19 
20 #define PROG_ID_MAX		7
21 
22 #define PROG_STATUS_MASK(id)	(1 << ((id) + 8))
23 #define PROG_PRES_MASK		0x7
24 #define PROG_PRES(layout, pckr)	((pckr >> layout->pres_shift) & PROG_PRES_MASK)
25 #define PROG_MAX_RM9200_CSS	3
26 
27 struct clk_programmable {
28 	struct clk_hw hw;
29 	struct regmap *regmap;
30 	u8 id;
31 	const struct clk_programmable_layout *layout;
32 };
33 
34 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)
35 
36 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,
37 						  unsigned long parent_rate)
38 {
39 	struct clk_programmable *prog = to_clk_programmable(hw);
40 	unsigned int pckr;
41 
42 	regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
43 
44 	return parent_rate >> PROG_PRES(prog->layout, pckr);
45 }
46 
47 static int clk_programmable_determine_rate(struct clk_hw *hw,
48 					   struct clk_rate_request *req)
49 {
50 	struct clk_hw *parent;
51 	long best_rate = -EINVAL;
52 	unsigned long parent_rate;
53 	unsigned long tmp_rate;
54 	int shift;
55 	int i;
56 
57 	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
58 		parent = clk_hw_get_parent_by_index(hw, i);
59 		if (!parent)
60 			continue;
61 
62 		parent_rate = clk_hw_get_rate(parent);
63 		for (shift = 0; shift < PROG_PRES_MASK; shift++) {
64 			tmp_rate = parent_rate >> shift;
65 			if (tmp_rate <= req->rate)
66 				break;
67 		}
68 
69 		if (tmp_rate > req->rate)
70 			continue;
71 
72 		if (best_rate < 0 ||
73 		    (req->rate - tmp_rate) < (req->rate - best_rate)) {
74 			best_rate = tmp_rate;
75 			req->best_parent_rate = parent_rate;
76 			req->best_parent_hw = parent;
77 		}
78 
79 		if (!best_rate)
80 			break;
81 	}
82 
83 	if (best_rate < 0)
84 		return best_rate;
85 
86 	req->rate = best_rate;
87 	return 0;
88 }
89 
90 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index)
91 {
92 	struct clk_programmable *prog = to_clk_programmable(hw);
93 	const struct clk_programmable_layout *layout = prog->layout;
94 	unsigned int mask = layout->css_mask;
95 	unsigned int pckr = index;
96 
97 	if (layout->have_slck_mck)
98 		mask |= AT91_PMC_CSSMCK_MCK;
99 
100 	if (index > layout->css_mask) {
101 		if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck)
102 			return -EINVAL;
103 
104 		pckr |= AT91_PMC_CSSMCK_MCK;
105 	}
106 
107 	regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr);
108 
109 	return 0;
110 }
111 
112 static u8 clk_programmable_get_parent(struct clk_hw *hw)
113 {
114 	struct clk_programmable *prog = to_clk_programmable(hw);
115 	const struct clk_programmable_layout *layout = prog->layout;
116 	unsigned int pckr;
117 	u8 ret;
118 
119 	regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
120 
121 	ret = pckr & layout->css_mask;
122 
123 	if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret)
124 		ret = PROG_MAX_RM9200_CSS + 1;
125 
126 	return ret;
127 }
128 
129 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
130 				     unsigned long parent_rate)
131 {
132 	struct clk_programmable *prog = to_clk_programmable(hw);
133 	const struct clk_programmable_layout *layout = prog->layout;
134 	unsigned long div = parent_rate / rate;
135 	unsigned int pckr;
136 	int shift = 0;
137 
138 	regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
139 
140 	if (!div)
141 		return -EINVAL;
142 
143 	shift = fls(div) - 1;
144 
145 	if (div != (1 << shift))
146 		return -EINVAL;
147 
148 	if (shift >= PROG_PRES_MASK)
149 		return -EINVAL;
150 
151 	regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id),
152 			   PROG_PRES_MASK << layout->pres_shift,
153 			   shift << layout->pres_shift);
154 
155 	return 0;
156 }
157 
158 static const struct clk_ops programmable_ops = {
159 	.recalc_rate = clk_programmable_recalc_rate,
160 	.determine_rate = clk_programmable_determine_rate,
161 	.get_parent = clk_programmable_get_parent,
162 	.set_parent = clk_programmable_set_parent,
163 	.set_rate = clk_programmable_set_rate,
164 };
165 
166 struct clk_hw * __init
167 at91_clk_register_programmable(struct regmap *regmap,
168 			       const char *name, const char **parent_names,
169 			       u8 num_parents, u8 id,
170 			       const struct clk_programmable_layout *layout)
171 {
172 	struct clk_programmable *prog;
173 	struct clk_hw *hw;
174 	struct clk_init_data init;
175 	int ret;
176 
177 	if (id > PROG_ID_MAX)
178 		return ERR_PTR(-EINVAL);
179 
180 	prog = kzalloc(sizeof(*prog), GFP_KERNEL);
181 	if (!prog)
182 		return ERR_PTR(-ENOMEM);
183 
184 	init.name = name;
185 	init.ops = &programmable_ops;
186 	init.parent_names = parent_names;
187 	init.num_parents = num_parents;
188 	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
189 
190 	prog->id = id;
191 	prog->layout = layout;
192 	prog->hw.init = &init;
193 	prog->regmap = regmap;
194 
195 	hw = &prog->hw;
196 	ret = clk_hw_register(NULL, &prog->hw);
197 	if (ret) {
198 		kfree(prog);
199 		hw = ERR_PTR(ret);
200 	} else {
201 		pmc_register_pck(id);
202 	}
203 
204 	return hw;
205 }
206 
207 const struct clk_programmable_layout at91rm9200_programmable_layout = {
208 	.pres_shift = 2,
209 	.css_mask = 0x3,
210 	.have_slck_mck = 0,
211 };
212 
213 const struct clk_programmable_layout at91sam9g45_programmable_layout = {
214 	.pres_shift = 2,
215 	.css_mask = 0x3,
216 	.have_slck_mck = 1,
217 };
218 
219 const struct clk_programmable_layout at91sam9x5_programmable_layout = {
220 	.pres_shift = 4,
221 	.css_mask = 0x7,
222 	.have_slck_mck = 0,
223 };
224