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