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