1// SPDX-License-Identifier: GPL-2.0-or-later
2/*
3 * Microchip Sparx5 SoC Clock driver.
4 *
5 * Copyright (c) 2019 Microchip Inc.
6 *
7 * Author: Lars Povlsen <lars.povlsen@microchip.com>
8 */
9
10#include <linux/io.h>
11#include <linux/module.h>
12#include <linux/clk-provider.h>
13#include <linux/bitfield.h>
14#include <linux/of.h>
15#include <linux/slab.h>
16#include <linux/platform_device.h>
17#include <dt-bindings/clock/microchip,sparx5.h>
18
19#define PLL_DIV		GENMASK(7, 0)
20#define PLL_PRE_DIV	GENMASK(10, 8)
21#define PLL_ROT_DIR	BIT(11)
22#define PLL_ROT_SEL	GENMASK(13, 12)
23#define PLL_ROT_ENA	BIT(14)
24#define PLL_CLK_ENA	BIT(15)
25
26#define MAX_SEL 4
27#define MAX_PRE BIT(3)
28
29static const u8 sel_rates[MAX_SEL] = { 0, 2*8, 2*4, 2*2 };
30
31static const char *clk_names[N_CLOCKS] = {
32	"core", "ddr", "cpu2", "arm2",
33	"aux1", "aux2", "aux3", "aux4",
34	"synce",
35};
36
37struct s5_hw_clk {
38	struct clk_hw hw;
39	void __iomem *reg;
40};
41
42struct s5_clk_data {
43	void __iomem *base;
44	struct s5_hw_clk s5_hw[N_CLOCKS];
45};
46
47struct s5_pll_conf {
48	unsigned long freq;
49	u8 div;
50	bool rot_ena;
51	u8 rot_sel;
52	u8 rot_dir;
53	u8 pre_div;
54};
55
56#define to_s5_pll(hw) container_of(hw, struct s5_hw_clk, hw)
57
58static unsigned long s5_calc_freq(unsigned long parent_rate,
59				  const struct s5_pll_conf *conf)
60{
61	unsigned long rate = parent_rate / conf->div;
62
63	if (conf->rot_ena) {
64		int sign = conf->rot_dir ? -1 : 1;
65		int divt = sel_rates[conf->rot_sel] * (1 + conf->pre_div);
66		int divb = divt + sign;
67
68		rate = mult_frac(rate, divt, divb);
69		rate = roundup(rate, 1000);
70	}
71
72	return rate;
73}
74
75static void s5_search_fractional(unsigned long rate,
76				 unsigned long parent_rate,
77				 int div,
78				 struct s5_pll_conf *conf)
79{
80	struct s5_pll_conf best;
81	ulong cur_offset, best_offset = rate;
82	int d, i, j;
83
84	memset(conf, 0, sizeof(*conf));
85	conf->div = div;
86	conf->rot_ena = 1;	/* Fractional rate */
87
88	for (d = 0; best_offset > 0 && d <= 1 ; d++) {
89		conf->rot_dir = !!d;
90		for (i = 0; best_offset > 0 && i < MAX_PRE; i++) {
91			conf->pre_div = i;
92			for (j = 1; best_offset > 0 && j < MAX_SEL; j++) {
93				conf->rot_sel = j;
94				conf->freq = s5_calc_freq(parent_rate, conf);
95				cur_offset = abs(rate - conf->freq);
96				if (cur_offset < best_offset) {
97					best_offset = cur_offset;
98					best = *conf;
99				}
100			}
101		}
102	}
103
104	/* Best match */
105	*conf = best;
106}
107
108static unsigned long s5_calc_params(unsigned long rate,
109				    unsigned long parent_rate,
110				    struct s5_pll_conf *conf)
111{
112	if (parent_rate % rate) {
113		struct s5_pll_conf alt1, alt2;
114		int div;
115
116		div = DIV_ROUND_CLOSEST_ULL(parent_rate, rate);
117		s5_search_fractional(rate, parent_rate, div, &alt1);
118
119		/* Straight match? */
120		if (alt1.freq == rate) {
121			*conf = alt1;
122		} else {
123			/* Try without rounding divider */
124			div = parent_rate / rate;
125			if (div != alt1.div) {
126				s5_search_fractional(rate, parent_rate, div,
127						     &alt2);
128				/* Select the better match */
129				if (abs(rate - alt1.freq) <
130				    abs(rate - alt2.freq))
131					*conf = alt1;
132				else
133					*conf = alt2;
134			}
135		}
136	} else {
137		/* Straight fit */
138		memset(conf, 0, sizeof(*conf));
139		conf->div = parent_rate / rate;
140	}
141
142	return conf->freq;
143}
144
145static int s5_pll_enable(struct clk_hw *hw)
146{
147	struct s5_hw_clk *pll = to_s5_pll(hw);
148	u32 val = readl(pll->reg);
149
150	val |= PLL_CLK_ENA;
151	writel(val, pll->reg);
152
153	return 0;
154}
155
156static void s5_pll_disable(struct clk_hw *hw)
157{
158	struct s5_hw_clk *pll = to_s5_pll(hw);
159	u32 val = readl(pll->reg);
160
161	val &= ~PLL_CLK_ENA;
162	writel(val, pll->reg);
163}
164
165static int s5_pll_set_rate(struct clk_hw *hw,
166			   unsigned long rate,
167			   unsigned long parent_rate)
168{
169	struct s5_hw_clk *pll = to_s5_pll(hw);
170	struct s5_pll_conf conf;
171	unsigned long eff_rate;
172	u32 val;
173
174	eff_rate = s5_calc_params(rate, parent_rate, &conf);
175	if (eff_rate != rate)
176		return -EOPNOTSUPP;
177
178	val = readl(pll->reg) & PLL_CLK_ENA;
179	val |= FIELD_PREP(PLL_DIV, conf.div);
180	if (conf.rot_ena) {
181		val |= PLL_ROT_ENA;
182		val |= FIELD_PREP(PLL_ROT_SEL, conf.rot_sel);
183		val |= FIELD_PREP(PLL_PRE_DIV, conf.pre_div);
184		if (conf.rot_dir)
185			val |= PLL_ROT_DIR;
186	}
187	writel(val, pll->reg);
188
189	return 0;
190}
191
192static unsigned long s5_pll_recalc_rate(struct clk_hw *hw,
193					unsigned long parent_rate)
194{
195	struct s5_hw_clk *pll = to_s5_pll(hw);
196	struct s5_pll_conf conf;
197	u32 val;
198
199	val = readl(pll->reg);
200
201	if (val & PLL_CLK_ENA) {
202		conf.div     = FIELD_GET(PLL_DIV, val);
203		conf.pre_div = FIELD_GET(PLL_PRE_DIV, val);
204		conf.rot_ena = FIELD_GET(PLL_ROT_ENA, val);
205		conf.rot_dir = FIELD_GET(PLL_ROT_DIR, val);
206		conf.rot_sel = FIELD_GET(PLL_ROT_SEL, val);
207
208		conf.freq = s5_calc_freq(parent_rate, &conf);
209	} else {
210		conf.freq = 0;
211	}
212
213	return conf.freq;
214}
215
216static long s5_pll_round_rate(struct clk_hw *hw, unsigned long rate,
217			      unsigned long *parent_rate)
218{
219	struct s5_pll_conf conf;
220
221	return s5_calc_params(rate, *parent_rate, &conf);
222}
223
224static const struct clk_ops s5_pll_ops = {
225	.enable		= s5_pll_enable,
226	.disable	= s5_pll_disable,
227	.set_rate	= s5_pll_set_rate,
228	.round_rate	= s5_pll_round_rate,
229	.recalc_rate	= s5_pll_recalc_rate,
230};
231
232static struct clk_hw *s5_clk_hw_get(struct of_phandle_args *clkspec, void *data)
233{
234	struct s5_clk_data *s5_clk = data;
235	unsigned int idx = clkspec->args[0];
236
237	if (idx >= N_CLOCKS) {
238		pr_err("%s: invalid index %u\n", __func__, idx);
239		return ERR_PTR(-EINVAL);
240	}
241
242	return &s5_clk->s5_hw[idx].hw;
243}
244
245static int s5_clk_probe(struct platform_device *pdev)
246{
247	struct device *dev = &pdev->dev;
248	int i, ret;
249	struct s5_clk_data *s5_clk;
250	struct clk_parent_data pdata = { .index = 0 };
251	struct clk_init_data init = {
252		.ops = &s5_pll_ops,
253		.num_parents = 1,
254		.parent_data = &pdata,
255	};
256
257	s5_clk = devm_kzalloc(dev, sizeof(*s5_clk), GFP_KERNEL);
258	if (!s5_clk)
259		return -ENOMEM;
260
261	s5_clk->base = devm_platform_ioremap_resource(pdev, 0);
262	if (IS_ERR(s5_clk->base))
263		return PTR_ERR(s5_clk->base);
264
265	for (i = 0; i < N_CLOCKS; i++) {
266		struct s5_hw_clk *s5_hw = &s5_clk->s5_hw[i];
267
268		init.name = clk_names[i];
269		s5_hw->reg = s5_clk->base + (i * 4);
270		s5_hw->hw.init = &init;
271		ret = devm_clk_hw_register(dev, &s5_hw->hw);
272		if (ret) {
273			dev_err(dev, "failed to register %s clock\n",
274				init.name);
275			return ret;
276		}
277	}
278
279	return devm_of_clk_add_hw_provider(dev, s5_clk_hw_get, s5_clk);
280}
281
282static const struct of_device_id s5_clk_dt_ids[] = {
283	{ .compatible = "microchip,sparx5-dpll", },
284	{ }
285};
286MODULE_DEVICE_TABLE(of, s5_clk_dt_ids);
287
288static struct platform_driver s5_clk_driver = {
289	.probe  = s5_clk_probe,
290	.driver = {
291		.name = "sparx5-clk",
292		.of_match_table = s5_clk_dt_ids,
293	},
294};
295builtin_platform_driver(s5_clk_driver);
296