1// SPDX-License-Identifier: GPL-2.0
2/*
3 * r8a7790 Common Clock Framework support
4 *
5 * Copyright (C) 2013  Renesas Solutions Corp.
6 *
7 * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
8 */
9
10#include <linux/clk-provider.h>
11#include <linux/init.h>
12#include <linux/io.h>
13#include <linux/kernel.h>
14#include <linux/notifier.h>
15#include <linux/of.h>
16#include <linux/of_address.h>
17#include <linux/pm.h>
18#include <linux/slab.h>
19
20#include "clk-div6.h"
21
22#define CPG_DIV6_CKSTP		BIT(8)
23#define CPG_DIV6_DIV(d)		((d) & 0x3f)
24#define CPG_DIV6_DIV_MASK	0x3f
25
26/**
27 * struct div6_clock - CPG 6 bit divider clock
28 * @hw: handle between common and hardware-specific interfaces
29 * @reg: IO-remapped register
30 * @div: divisor value (1-64)
31 * @src_mask: Bitmask covering the register bits to select the parent clock
32 * @nb: Notifier block to save/restore clock state for system resume
33 * @parents: Array to map from valid parent clocks indices to hardware indices
34 */
35struct div6_clock {
36	struct clk_hw hw;
37	void __iomem *reg;
38	unsigned int div;
39	u32 src_mask;
40	struct notifier_block nb;
41	u8 parents[];
42};
43
44#define to_div6_clock(_hw) container_of(_hw, struct div6_clock, hw)
45
46static int cpg_div6_clock_enable(struct clk_hw *hw)
47{
48	struct div6_clock *clock = to_div6_clock(hw);
49	u32 val;
50
51	val = (readl(clock->reg) & ~(CPG_DIV6_DIV_MASK | CPG_DIV6_CKSTP))
52	    | CPG_DIV6_DIV(clock->div - 1);
53	writel(val, clock->reg);
54
55	return 0;
56}
57
58static void cpg_div6_clock_disable(struct clk_hw *hw)
59{
60	struct div6_clock *clock = to_div6_clock(hw);
61	u32 val;
62
63	val = readl(clock->reg);
64	val |= CPG_DIV6_CKSTP;
65	/*
66	 * DIV6 clocks require the divisor field to be non-zero when stopping
67	 * the clock. However, some clocks (e.g. ZB on sh73a0) fail to be
68	 * re-enabled later if the divisor field is changed when stopping the
69	 * clock
70	 */
71	if (!(val & CPG_DIV6_DIV_MASK))
72		val |= CPG_DIV6_DIV_MASK;
73	writel(val, clock->reg);
74}
75
76static int cpg_div6_clock_is_enabled(struct clk_hw *hw)
77{
78	struct div6_clock *clock = to_div6_clock(hw);
79
80	return !(readl(clock->reg) & CPG_DIV6_CKSTP);
81}
82
83static unsigned long cpg_div6_clock_recalc_rate(struct clk_hw *hw,
84						unsigned long parent_rate)
85{
86	struct div6_clock *clock = to_div6_clock(hw);
87
88	return parent_rate / clock->div;
89}
90
91static unsigned int cpg_div6_clock_calc_div(unsigned long rate,
92					    unsigned long parent_rate)
93{
94	unsigned int div;
95
96	if (!rate)
97		rate = 1;
98
99	div = DIV_ROUND_CLOSEST(parent_rate, rate);
100	return clamp(div, 1U, 64U);
101}
102
103static int cpg_div6_clock_determine_rate(struct clk_hw *hw,
104					 struct clk_rate_request *req)
105{
106	unsigned long prate, calc_rate, diff, best_rate, best_prate;
107	unsigned int num_parents = clk_hw_get_num_parents(hw);
108	struct clk_hw *parent, *best_parent = NULL;
109	unsigned int i, min_div, max_div, div;
110	unsigned long min_diff = ULONG_MAX;
111
112	for (i = 0; i < num_parents; i++) {
113		parent = clk_hw_get_parent_by_index(hw, i);
114		if (!parent)
115			continue;
116
117		prate = clk_hw_get_rate(parent);
118		if (!prate)
119			continue;
120
121		min_div = max(DIV_ROUND_UP(prate, req->max_rate), 1UL);
122		max_div = req->min_rate ? min(prate / req->min_rate, 64UL) : 64;
123		if (max_div < min_div)
124			continue;
125
126		div = cpg_div6_clock_calc_div(req->rate, prate);
127		div = clamp(div, min_div, max_div);
128		calc_rate = prate / div;
129		diff = calc_rate > req->rate ? calc_rate - req->rate
130					     : req->rate - calc_rate;
131		if (diff < min_diff) {
132			best_rate = calc_rate;
133			best_parent = parent;
134			best_prate = prate;
135			min_diff = diff;
136		}
137	}
138
139	if (!best_parent)
140		return -EINVAL;
141
142	req->best_parent_rate = best_prate;
143	req->best_parent_hw = best_parent;
144	req->rate = best_rate;
145	return 0;
146}
147
148static int cpg_div6_clock_set_rate(struct clk_hw *hw, unsigned long rate,
149				   unsigned long parent_rate)
150{
151	struct div6_clock *clock = to_div6_clock(hw);
152	unsigned int div = cpg_div6_clock_calc_div(rate, parent_rate);
153	u32 val;
154
155	clock->div = div;
156
157	val = readl(clock->reg) & ~CPG_DIV6_DIV_MASK;
158	/* Only program the new divisor if the clock isn't stopped. */
159	if (!(val & CPG_DIV6_CKSTP))
160		writel(val | CPG_DIV6_DIV(clock->div - 1), clock->reg);
161
162	return 0;
163}
164
165static u8 cpg_div6_clock_get_parent(struct clk_hw *hw)
166{
167	struct div6_clock *clock = to_div6_clock(hw);
168	unsigned int i;
169	u8 hw_index;
170
171	if (clock->src_mask == 0)
172		return 0;
173
174	hw_index = (readl(clock->reg) & clock->src_mask) >>
175		   __ffs(clock->src_mask);
176	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
177		if (clock->parents[i] == hw_index)
178			return i;
179	}
180
181	pr_err("%s: %s DIV6 clock set to invalid parent %u\n",
182	       __func__, clk_hw_get_name(hw), hw_index);
183	return 0;
184}
185
186static int cpg_div6_clock_set_parent(struct clk_hw *hw, u8 index)
187{
188	struct div6_clock *clock = to_div6_clock(hw);
189	u32 src;
190
191	if (index >= clk_hw_get_num_parents(hw))
192		return -EINVAL;
193
194	src = clock->parents[index] << __ffs(clock->src_mask);
195	writel((readl(clock->reg) & ~clock->src_mask) | src, clock->reg);
196	return 0;
197}
198
199static const struct clk_ops cpg_div6_clock_ops = {
200	.enable = cpg_div6_clock_enable,
201	.disable = cpg_div6_clock_disable,
202	.is_enabled = cpg_div6_clock_is_enabled,
203	.get_parent = cpg_div6_clock_get_parent,
204	.set_parent = cpg_div6_clock_set_parent,
205	.recalc_rate = cpg_div6_clock_recalc_rate,
206	.determine_rate = cpg_div6_clock_determine_rate,
207	.set_rate = cpg_div6_clock_set_rate,
208};
209
210static int cpg_div6_clock_notifier_call(struct notifier_block *nb,
211					unsigned long action, void *data)
212{
213	struct div6_clock *clock = container_of(nb, struct div6_clock, nb);
214
215	switch (action) {
216	case PM_EVENT_RESUME:
217		/*
218		 * TODO: This does not yet support DIV6 clocks with multiple
219		 * parents, as the parent selection bits are not restored.
220		 * Fortunately so far such DIV6 clocks are found only on
221		 * R/SH-Mobile SoCs, while the resume functionality is only
222		 * needed on R-Car Gen3.
223		 */
224		if (__clk_get_enable_count(clock->hw.clk))
225			cpg_div6_clock_enable(&clock->hw);
226		else
227			cpg_div6_clock_disable(&clock->hw);
228		return NOTIFY_OK;
229	}
230
231	return NOTIFY_DONE;
232}
233
234/**
235 * cpg_div6_register - Register a DIV6 clock
236 * @name: Name of the DIV6 clock
237 * @num_parents: Number of parent clocks of the DIV6 clock (1, 4, or 8)
238 * @parent_names: Array containing the names of the parent clocks
239 * @reg: Mapped register used to control the DIV6 clock
240 * @notifiers: Optional notifier chain to save/restore state for system resume
241 */
242struct clk * __init cpg_div6_register(const char *name,
243				      unsigned int num_parents,
244				      const char **parent_names,
245				      void __iomem *reg,
246				      struct raw_notifier_head *notifiers)
247{
248	unsigned int valid_parents;
249	struct clk_init_data init = {};
250	struct div6_clock *clock;
251	struct clk *clk;
252	unsigned int i;
253
254	clock = kzalloc(struct_size(clock, parents, num_parents), GFP_KERNEL);
255	if (!clock)
256		return ERR_PTR(-ENOMEM);
257
258	clock->reg = reg;
259
260	/*
261	 * Read the divisor. Disabling the clock overwrites the divisor, so we
262	 * need to cache its value for the enable operation.
263	 */
264	clock->div = (readl(clock->reg) & CPG_DIV6_DIV_MASK) + 1;
265
266	switch (num_parents) {
267	case 1:
268		/* fixed parent clock */
269		clock->src_mask = 0;
270		break;
271	case 4:
272		/* clock with EXSRC bits 6-7 */
273		clock->src_mask = GENMASK(7, 6);
274		break;
275	case 8:
276		/* VCLK with EXSRC bits 12-14 */
277		clock->src_mask = GENMASK(14, 12);
278		break;
279	default:
280		pr_err("%s: invalid number of parents for DIV6 clock %s\n",
281		       __func__, name);
282		clk = ERR_PTR(-EINVAL);
283		goto free_clock;
284	}
285
286	/* Filter out invalid parents */
287	for (i = 0, valid_parents = 0; i < num_parents; i++) {
288		if (parent_names[i]) {
289			parent_names[valid_parents] = parent_names[i];
290			clock->parents[valid_parents] = i;
291			valid_parents++;
292		}
293	}
294
295	/* Register the clock. */
296	init.name = name;
297	init.ops = &cpg_div6_clock_ops;
298	init.parent_names = parent_names;
299	init.num_parents = valid_parents;
300
301	clock->hw.init = &init;
302
303	clk = clk_register(NULL, &clock->hw);
304	if (IS_ERR(clk))
305		goto free_clock;
306
307	if (notifiers) {
308		clock->nb.notifier_call = cpg_div6_clock_notifier_call;
309		raw_notifier_chain_register(notifiers, &clock->nb);
310	}
311
312	return clk;
313
314free_clock:
315	kfree(clock);
316	return clk;
317}
318
319static void __init cpg_div6_clock_init(struct device_node *np)
320{
321	unsigned int num_parents;
322	const char **parent_names;
323	const char *clk_name = np->name;
324	void __iomem *reg;
325	struct clk *clk;
326	unsigned int i;
327
328	num_parents = of_clk_get_parent_count(np);
329	if (num_parents < 1) {
330		pr_err("%s: no parent found for %pOFn DIV6 clock\n",
331		       __func__, np);
332		return;
333	}
334
335	parent_names = kmalloc_array(num_parents, sizeof(*parent_names),
336				GFP_KERNEL);
337	if (!parent_names)
338		return;
339
340	reg = of_iomap(np, 0);
341	if (reg == NULL) {
342		pr_err("%s: failed to map %pOFn DIV6 clock register\n",
343		       __func__, np);
344		goto error;
345	}
346
347	/* Parse the DT properties. */
348	of_property_read_string(np, "clock-output-names", &clk_name);
349
350	for (i = 0; i < num_parents; i++)
351		parent_names[i] = of_clk_get_parent_name(np, i);
352
353	clk = cpg_div6_register(clk_name, num_parents, parent_names, reg, NULL);
354	if (IS_ERR(clk)) {
355		pr_err("%s: failed to register %pOFn DIV6 clock (%ld)\n",
356		       __func__, np, PTR_ERR(clk));
357		goto error;
358	}
359
360	of_clk_add_provider(np, of_clk_src_simple_get, clk);
361
362	kfree(parent_names);
363	return;
364
365error:
366	if (reg)
367		iounmap(reg);
368	kfree(parent_names);
369}
370CLK_OF_DECLARE(cpg_div6_clk, "renesas,cpg-div6-clock", cpg_div6_clock_init);
371