clk-div6.c

  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/bitfield.h>
 11#include <linux/clk-provider.h>
 12#include <linux/init.h>
 13#include <linux/io.h>
 14#include <linux/kernel.h>
 15#include <linux/notifier.h>
 16#include <linux/of.h>
 17#include <linux/of_address.h>
 18#include <linux/pm.h>
 19#include <linux/slab.h>
 20
 21#include "clk-div6.h"
 22
 23#define CPG_DIV6_CKSTP		BIT(8)
 24#define CPG_DIV6_DIV(d)		((d) & 0x3f)
 25#define CPG_DIV6_DIV_MASK	0x3f
 26
 27/**
 28 * struct div6_clock - CPG 6 bit divider clock
 29 * @hw: handle between common and hardware-specific interfaces
 30 * @reg: IO-remapped register
 31 * @div: divisor value (1-64)
 32 * @src_mask: Bitmask covering the register bits to select the parent clock
 33 * @nb: Notifier block to save/restore clock state for system resume
 34 * @parents: Array to map from valid parent clocks indices to hardware indices
 35 */
 36struct div6_clock {
 37	struct clk_hw hw;
 38	void __iomem *reg;
 39	unsigned int div;
 40	u32 src_mask;
 41	struct notifier_block nb;
 42	u8 parents[];
 43};
 44
 45#define to_div6_clock(_hw) container_of(_hw, struct div6_clock, hw)
 46
 47static int cpg_div6_clock_enable(struct clk_hw *hw)
 48{
 49	struct div6_clock *clock = to_div6_clock(hw);
 50	u32 val;
 51
 52	val = (readl(clock->reg) & ~(CPG_DIV6_DIV_MASK | CPG_DIV6_CKSTP))
 53	    | CPG_DIV6_DIV(clock->div - 1);
 54	writel(val, clock->reg);
 55
 56	return 0;
 57}
 58
 59static void cpg_div6_clock_disable(struct clk_hw *hw)
 60{
 61	struct div6_clock *clock = to_div6_clock(hw);
 62	u32 val;
 63
 64	val = readl(clock->reg);
 65	val |= CPG_DIV6_CKSTP;
 66	/*
 67	 * DIV6 clocks require the divisor field to be non-zero when stopping
 68	 * the clock. However, some clocks (e.g. ZB on sh73a0) fail to be
 69	 * re-enabled later if the divisor field is changed when stopping the
 70	 * clock
 71	 */
 72	if (!(val & CPG_DIV6_DIV_MASK))
 73		val |= CPG_DIV6_DIV_MASK;
 74	writel(val, clock->reg);
 75}
 76
 77static int cpg_div6_clock_is_enabled(struct clk_hw *hw)
 78{
 79	struct div6_clock *clock = to_div6_clock(hw);
 80
 81	return !(readl(clock->reg) & CPG_DIV6_CKSTP);
 82}
 83
 84static unsigned long cpg_div6_clock_recalc_rate(struct clk_hw *hw,
 85						unsigned long parent_rate)
 86{
 87	struct div6_clock *clock = to_div6_clock(hw);
 88
 89	return parent_rate / clock->div;
 90}
 91
 92static unsigned int cpg_div6_clock_calc_div(unsigned long rate,
 93					    unsigned long parent_rate)
 94{
 95	unsigned int div;
 96
 97	if (!rate)
 98		rate = 1;
 99
100	div = DIV_ROUND_CLOSEST(parent_rate, rate);
101	return clamp(div, 1U, 64U);
102}
103
104static int cpg_div6_clock_determine_rate(struct clk_hw *hw,
105					 struct clk_rate_request *req)
106{
107	unsigned long prate, calc_rate, diff, best_rate, best_prate;
108	unsigned int num_parents = clk_hw_get_num_parents(hw);
109	struct clk_hw *parent, *best_parent = NULL;
110	unsigned int i, min_div, max_div, div;
111	unsigned long min_diff = ULONG_MAX;
112
113	for (i = 0; i < num_parents; i++) {
114		parent = clk_hw_get_parent_by_index(hw, i);
115		if (!parent)
116			continue;
117
118		prate = clk_hw_get_rate(parent);
119		if (!prate)
120			continue;
121
122		min_div = max(DIV_ROUND_UP(prate, req->max_rate), 1UL);
123		max_div = req->min_rate ? min(prate / req->min_rate, 64UL) : 64;
124		if (max_div < min_div)
125			continue;
126
127		div = cpg_div6_clock_calc_div(req->rate, prate);
128		div = clamp(div, min_div, max_div);
129		calc_rate = prate / div;
130		diff = calc_rate > req->rate ? calc_rate - req->rate
131					     : req->rate - calc_rate;
132		if (diff < min_diff) {
133			best_rate = calc_rate;
134			best_parent = parent;
135			best_prate = prate;
136			min_diff = diff;
137		}
138	}
139
140	if (!best_parent)
141		return -EINVAL;
142
143	req->best_parent_rate = best_prate;
144	req->best_parent_hw = best_parent;
145	req->rate = best_rate;
146	return 0;
147}
148
149static int cpg_div6_clock_set_rate(struct clk_hw *hw, unsigned long rate,
150				   unsigned long parent_rate)
151{
152	struct div6_clock *clock = to_div6_clock(hw);
153	unsigned int div = cpg_div6_clock_calc_div(rate, parent_rate);
154	u32 val;
155
156	clock->div = div;
157
158	val = readl(clock->reg) & ~CPG_DIV6_DIV_MASK;
159	/* Only program the new divisor if the clock isn't stopped. */
160	if (!(val & CPG_DIV6_CKSTP))
161		writel(val | CPG_DIV6_DIV(clock->div - 1), clock->reg);
162
163	return 0;
164}
165
166static u8 cpg_div6_clock_get_parent(struct clk_hw *hw)
167{
168	struct div6_clock *clock = to_div6_clock(hw);
169	unsigned int i;
170	u8 hw_index;
171
172	if (clock->src_mask == 0)
173		return 0;
174
175	hw_index = field_get(clock->src_mask, readl(clock->reg));
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 = field_prep(clock->src_mask, clock->parents[index]);
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);