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