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/bitops.h>
  7#include <linux/clk-provider.h>
  8#include <linux/clkdev.h>
  9#include <linux/clk/at91_pmc.h>
 10#include <linux/of.h>
 11#include <linux/mfd/syscon.h>
 12#include <linux/regmap.h>
 13
 14#include "pmc.h"
 15
 16DEFINE_SPINLOCK(pmc_pcr_lock);
 17
 18#define PERIPHERAL_ID_MIN	2
 19#define PERIPHERAL_ID_MAX	31
 20#define PERIPHERAL_MASK(id)	(1 << ((id) & PERIPHERAL_ID_MAX))
 21
 22#define PERIPHERAL_MAX_SHIFT	3
 23
 24struct clk_peripheral {
 25	struct clk_hw hw;
 26	struct regmap *regmap;
 27	u32 id;
 28};
 29
 30#define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw)
 31
 32struct clk_sam9x5_peripheral {
 33	struct clk_hw hw;
 34	struct regmap *regmap;
 35	struct clk_range range;
 36	spinlock_t *lock;
 37	u32 id;
 38	u32 div;
 39	const struct clk_pcr_layout *layout;
 40	bool auto_div;
 41};
 42
 43#define to_clk_sam9x5_peripheral(hw) \
 44	container_of(hw, struct clk_sam9x5_peripheral, hw)
 45
 46static int clk_peripheral_enable(struct clk_hw *hw)
 47{
 48	struct clk_peripheral *periph = to_clk_peripheral(hw);
 49	int offset = AT91_PMC_PCER;
 50	u32 id = periph->id;
 51
 52	if (id < PERIPHERAL_ID_MIN)
 53		return 0;
 54	if (id > PERIPHERAL_ID_MAX)
 55		offset = AT91_PMC_PCER1;
 56	regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
 57
 58	return 0;
 59}
 60
 61static void clk_peripheral_disable(struct clk_hw *hw)
 62{
 63	struct clk_peripheral *periph = to_clk_peripheral(hw);
 64	int offset = AT91_PMC_PCDR;
 65	u32 id = periph->id;
 66
 67	if (id < PERIPHERAL_ID_MIN)
 68		return;
 69	if (id > PERIPHERAL_ID_MAX)
 70		offset = AT91_PMC_PCDR1;
 71	regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
 72}
 73
 74static int clk_peripheral_is_enabled(struct clk_hw *hw)
 75{
 76	struct clk_peripheral *periph = to_clk_peripheral(hw);
 77	int offset = AT91_PMC_PCSR;
 78	unsigned int status;
 79	u32 id = periph->id;
 80
 81	if (id < PERIPHERAL_ID_MIN)
 82		return 1;
 83	if (id > PERIPHERAL_ID_MAX)
 84		offset = AT91_PMC_PCSR1;
 85	regmap_read(periph->regmap, offset, &status);
 86
 87	return status & PERIPHERAL_MASK(id) ? 1 : 0;
 88}
 89
 90static const struct clk_ops peripheral_ops = {
 91	.enable = clk_peripheral_enable,
 92	.disable = clk_peripheral_disable,
 93	.is_enabled = clk_peripheral_is_enabled,
 94};
 95
 96struct clk_hw * __init
 97at91_clk_register_peripheral(struct regmap *regmap, const char *name,
 98			     const char *parent_name, u32 id)
 99{
100	struct clk_peripheral *periph;
101	struct clk_init_data init;
102	struct clk_hw *hw;
103	int ret;
104
105	if (!name || !parent_name || id > PERIPHERAL_ID_MAX)
106		return ERR_PTR(-EINVAL);
107
108	periph = kzalloc(sizeof(*periph), GFP_KERNEL);
109	if (!periph)
110		return ERR_PTR(-ENOMEM);
111
112	init.name = name;
113	init.ops = &peripheral_ops;
114	init.parent_names = (parent_name ? &parent_name : NULL);
115	init.num_parents = (parent_name ? 1 : 0);
116	init.flags = 0;
117
118	periph->id = id;
119	periph->hw.init = &init;
120	periph->regmap = regmap;
121
122	hw = &periph->hw;
123	ret = clk_hw_register(NULL, &periph->hw);
124	if (ret) {
125		kfree(periph);
126		hw = ERR_PTR(ret);
127	}
128
129	return hw;
130}
131
132static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
133{
134	struct clk_hw *parent;
135	unsigned long parent_rate;
136	int shift = 0;
137
138	if (!periph->auto_div)
139		return;
140
141	if (periph->range.max) {
142		parent = clk_hw_get_parent_by_index(&periph->hw, 0);
143		parent_rate = clk_hw_get_rate(parent);
144		if (!parent_rate)
145			return;
146
147		for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
148			if (parent_rate >> shift <= periph->range.max)
149				break;
150		}
151	}
152
153	periph->auto_div = false;
154	periph->div = shift;
155}
156
157static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
158{
159	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
160	unsigned long flags;
161
162	if (periph->id < PERIPHERAL_ID_MIN)
163		return 0;
164
165	spin_lock_irqsave(periph->lock, flags);
166	regmap_write(periph->regmap, periph->layout->offset,
167		     (periph->id & periph->layout->pid_mask));
168	regmap_update_bits(periph->regmap, periph->layout->offset,
169			   periph->layout->div_mask | periph->layout->cmd |
170			   AT91_PMC_PCR_EN,
171			   field_prep(periph->layout->div_mask, periph->div) |
172			   periph->layout->cmd |
173			   AT91_PMC_PCR_EN);
174	spin_unlock_irqrestore(periph->lock, flags);
175
176	return 0;
177}
178
179static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
180{
181	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
182	unsigned long flags;
183
184	if (periph->id < PERIPHERAL_ID_MIN)
185		return;
186
187	spin_lock_irqsave(periph->lock, flags);
188	regmap_write(periph->regmap, periph->layout->offset,
189		     (periph->id & periph->layout->pid_mask));
190	regmap_update_bits(periph->regmap, periph->layout->offset,
191			   AT91_PMC_PCR_EN | periph->layout->cmd,
192			   periph->layout->cmd);
193	spin_unlock_irqrestore(periph->lock, flags);
194}
195
196static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw)
197{
198	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
199	unsigned long flags;
200	unsigned int status;
201
202	if (periph->id < PERIPHERAL_ID_MIN)
203		return 1;
204
205	spin_lock_irqsave(periph->lock, flags);
206	regmap_write(periph->regmap, periph->layout->offset,
207		     (periph->id & periph->layout->pid_mask));
208	regmap_read(periph->regmap, periph->layout->offset, &status);
209	spin_unlock_irqrestore(periph->lock, flags);
210
211	return status & AT91_PMC_PCR_EN ? 1 : 0;
212}
213
214static unsigned long
215clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw,
216				  unsigned long parent_rate)
217{
218	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
219	unsigned long flags;
220	unsigned int status;
221
222	if (periph->id < PERIPHERAL_ID_MIN)
223		return parent_rate;
224
225	spin_lock_irqsave(periph->lock, flags);
226	regmap_write(periph->regmap, periph->layout->offset,
227		     (periph->id & periph->layout->pid_mask));
228	regmap_read(periph->regmap, periph->layout->offset, &status);
229	spin_unlock_irqrestore(periph->lock, flags);
230
231	if (status & AT91_PMC_PCR_EN) {
232		periph->div = field_get(periph->layout->div_mask, status);
233		periph->auto_div = false;
234	} else {
235		clk_sam9x5_peripheral_autodiv(periph);
236	}
237
238	return parent_rate >> periph->div;
239}
240
241static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
242					     unsigned long rate,
243					     unsigned long *parent_rate)
244{
245	int shift = 0;
246	unsigned long best_rate;
247	unsigned long best_diff;
248	unsigned long cur_rate = *parent_rate;
249	unsigned long cur_diff;
250	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
251
252	if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
253		return *parent_rate;
254
255	if (periph->range.max) {
256		for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
257			cur_rate = *parent_rate >> shift;
258			if (cur_rate <= periph->range.max)
259				break;
260		}
261	}
262
263	if (rate >= cur_rate)
264		return cur_rate;
265
266	best_diff = cur_rate - rate;
267	best_rate = cur_rate;
268	for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
269		cur_rate = *parent_rate >> shift;
270		if (cur_rate < rate)
271			cur_diff = rate - cur_rate;
272		else
273			cur_diff = cur_rate - rate;
274
275		if (cur_diff < best_diff) {
276			best_diff = cur_diff;
277			best_rate = cur_rate;
278		}
279
280		if (!best_diff || cur_rate < rate)
281			break;
282	}
283
284	return best_rate;
285}
286
287static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
288					  unsigned long rate,
289					  unsigned long parent_rate)
290{
291	int shift;
292	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
293	if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
294		if (parent_rate == rate)
295			return 0;
296		else
297			return -EINVAL;
298	}
299
300	if (periph->range.max && rate > periph->range.max)
301		return -EINVAL;
302
303	for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
304		if (parent_rate >> shift == rate) {
305			periph->auto_div = false;
306			periph->div = shift;
307			return 0;
308		}
309	}
310
311	return -EINVAL;
312}
313
314static const struct clk_ops sam9x5_peripheral_ops = {
315	.enable = clk_sam9x5_peripheral_enable,
316	.disable = clk_sam9x5_peripheral_disable,
317	.is_enabled = clk_sam9x5_peripheral_is_enabled,
318	.recalc_rate = clk_sam9x5_peripheral_recalc_rate,
319	.round_rate = clk_sam9x5_peripheral_round_rate,
320	.set_rate = clk_sam9x5_peripheral_set_rate,
321};
322
323struct clk_hw * __init
324at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
325				    const struct clk_pcr_layout *layout,
326				    const char *name, const char *parent_name,
327				    u32 id, const struct clk_range *range)
328{
329	struct clk_sam9x5_peripheral *periph;
330	struct clk_init_data init;
331	struct clk_hw *hw;
332	int ret;
333
334	if (!name || !parent_name)
335		return ERR_PTR(-EINVAL);
336
337	periph = kzalloc(sizeof(*periph), GFP_KERNEL);
338	if (!periph)
339		return ERR_PTR(-ENOMEM);
340
341	init.name = name;
342	init.ops = &sam9x5_peripheral_ops;
343	init.parent_names = (parent_name ? &parent_name : NULL);
344	init.num_parents = (parent_name ? 1 : 0);
345	init.flags = 0;
346
347	periph->id = id;
348	periph->hw.init = &init;
349	periph->div = 0;
350	periph->regmap = regmap;
351	periph->lock = lock;
352	if (layout->div_mask)
353		periph->auto_div = true;
354	periph->layout = layout;
355	periph->range = *range;
356
357	hw = &periph->hw;
358	ret = clk_hw_register(NULL, &periph->hw);
359	if (ret) {
360		kfree(periph);
361		hw = ERR_PTR(ret);
362	} else {
363		clk_sam9x5_peripheral_autodiv(periph);
364		pmc_register_id(id);
365	}
366
367	return hw;
368}