forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2023-12-11 d2ccde1c8e90d38cee87a1b0309ad2827f3fd30d 
 kernel/drivers/clk/at91/clk-master.c
....@@ -1,11 +1,6 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
12 /*
23  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
3
- *
4
- * This program is free software; you can redistribute it and/or modify
5
- * it under the terms of the GNU General Public License as published by
6
- * the Free Software Foundation; either version 2 of the License, or
7
- * (at your option) any later version.
8
- *
94  */
105 
116 #include <linux/clk-provider.h>
....@@ -17,47 +12,54 @@
1712 
1813 #include "pmc.h"
1914 
20
-#define MASTER_SOURCE_MAX	4
21
-
2215 #define MASTER_PRES_MASK	0x7
2316 #define MASTER_PRES_MAX		MASTER_PRES_MASK
2417 #define MASTER_DIV_SHIFT	8
2518 #define MASTER_DIV_MASK		0x3
2619 
27
-struct clk_master_characteristics {
28
-	struct clk_range output;
29
-	u32 divisors[4];
30
-	u8 have_div3_pres;
31
-};
20
+#define PMC_MCR			0x30
21
+#define PMC_MCR_ID_MSK		GENMASK(3, 0)
22
+#define PMC_MCR_CMD		BIT(7)
23
+#define PMC_MCR_DIV		GENMASK(10, 8)
24
+#define PMC_MCR_CSS		GENMASK(20, 16)
25
+#define PMC_MCR_CSS_SHIFT	(16)
26
+#define PMC_MCR_EN		BIT(28)
3227 
33
-struct clk_master_layout {
34
-	u32 mask;
35
-	u8 pres_shift;
36
-};
28
+#define PMC_MCR_ID(x)		((x) & PMC_MCR_ID_MSK)
29
+
30
+#define MASTER_MAX_ID		4
3731 
3832 #define to_clk_master(hw) container_of(hw, struct clk_master, hw)
3933 
4034 struct clk_master {
4135 	struct clk_hw hw;
4236 	struct regmap *regmap;
37
+	spinlock_t *lock;
4338 	const struct clk_master_layout *layout;
4439 	const struct clk_master_characteristics *characteristics;
40
+	u32 *mux_table;
41
+	u32 mckr;
42
+	int chg_pid;
43
+	u8 id;
44
+	u8 parent;
45
+	u8 div;
4546 };
4647 
47
-static inline bool clk_master_ready(struct regmap *regmap)
48
+static inline bool clk_master_ready(struct clk_master *master)
4849 {
50
+	unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
4951 	unsigned int status;
5052 
51
-	regmap_read(regmap, AT91_PMC_SR, &status);
53
+	regmap_read(master->regmap, AT91_PMC_SR, &status);
5254 
53
-	return status & AT91_PMC_MCKRDY ? 1 : 0;
55
+	return !!(status & bit);
5456 }
5557 
5658 static int clk_master_prepare(struct clk_hw *hw)
5759 {
5860 	struct clk_master *master = to_clk_master(hw);
5961 
60
-	while (!clk_master_ready(master->regmap))
62
+	while (!clk_master_ready(master))
6163 		cpu_relax();
6264 
6365 	return 0;
....@@ -67,7 +69,7 @@
6769 {
6870 	struct clk_master *master = to_clk_master(hw);
6971 
70
-	return clk_master_ready(master->regmap);
72
+	return clk_master_ready(master);
7173 }
7274 
7375 static unsigned long clk_master_recalc_rate(struct clk_hw *hw,
....@@ -82,7 +84,7 @@
8284 						master->characteristics;
8385 	unsigned int mckr;
8486 
85
-	regmap_read(master->regmap, AT91_PMC_MCKR, &mckr);
87
+	regmap_read(master->regmap, master->layout->offset, &mckr);
8688 	mckr &= layout->mask;
8789 
8890 	pres = (mckr >> layout->pres_shift) & MASTER_PRES_MASK;
....@@ -108,7 +110,7 @@
108110 	struct clk_master *master = to_clk_master(hw);
109111 	unsigned int mckr;
110112 
111
-	regmap_read(master->regmap, AT91_PMC_MCKR, &mckr);
113
+	regmap_read(master->regmap, master->layout->offset, &mckr);
112114 
113115 	return mckr & AT91_PMC_CSS;
114116 }
....@@ -120,7 +122,7 @@
120122 	.get_parent = clk_master_get_parent,
121123 };
122124 
123
-static struct clk_hw * __init
125
+struct clk_hw * __init
124126 at91_clk_register_master(struct regmap *regmap,
125127 		const char *name, int num_parents,
126128 		const char **parent_names,
....@@ -128,7 +130,7 @@
128130 		const struct clk_master_characteristics *characteristics)
129131 {
130132 	struct clk_master *master;
131
-	struct clk_init_data init = {};
133
+	struct clk_init_data init;
132134 	struct clk_hw *hw;
133135 	int ret;
134136 
....@@ -160,93 +162,295 @@
160162 	return hw;
161163 }
162164 
165
+static unsigned long
166
+clk_sama7g5_master_recalc_rate(struct clk_hw *hw,
167
+			       unsigned long parent_rate)
168
+{
169
+	struct clk_master *master = to_clk_master(hw);
163170 
164
-static const struct clk_master_layout at91rm9200_master_layout = {
171
+	return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
172
+}
173
+
174
+static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
175
+					 struct clk_hw *parent,
176
+					 unsigned long parent_rate,
177
+					 long *best_rate,
178
+					 long *best_diff,
179
+					 u32 div)
180
+{
181
+	unsigned long tmp_rate, tmp_diff;
182
+
183
+	if (div == MASTER_PRES_MAX)
184
+		tmp_rate = parent_rate / 3;
185
+	else
186
+		tmp_rate = parent_rate >> div;
187
+
188
+	tmp_diff = abs(req->rate - tmp_rate);
189
+
190
+	if (*best_diff < 0 || *best_diff >= tmp_diff) {
191
+		*best_rate = tmp_rate;
192
+		*best_diff = tmp_diff;
193
+		req->best_parent_rate = parent_rate;
194
+		req->best_parent_hw = parent;
195
+	}
196
+}
197
+
198
+static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
199
+					     struct clk_rate_request *req)
200
+{
201
+	struct clk_master *master = to_clk_master(hw);
202
+	struct clk_rate_request req_parent = *req;
203
+	struct clk_hw *parent;
204
+	long best_rate = LONG_MIN, best_diff = LONG_MIN;
205
+	unsigned long parent_rate;
206
+	unsigned int div, i;
207
+
208
+	/* First: check the dividers of MCR. */
209
+	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
210
+		parent = clk_hw_get_parent_by_index(hw, i);
211
+		if (!parent)
212
+			continue;
213
+
214
+		parent_rate = clk_hw_get_rate(parent);
215
+		if (!parent_rate)
216
+			continue;
217
+
218
+		for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
219
+			clk_sama7g5_master_best_diff(req, parent, parent_rate,
220
+						     &best_rate, &best_diff,
221
+						     div);
222
+			if (!best_diff)
223
+				break;
224
+		}
225
+
226
+		if (!best_diff)
227
+			break;
228
+	}
229
+
230
+	/* Second: try to request rate form changeable parent. */
231
+	if (master->chg_pid < 0)
232
+		goto end;
233
+
234
+	parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
235
+	if (!parent)
236
+		goto end;
237
+
238
+	for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
239
+		if (div == MASTER_PRES_MAX)
240
+			req_parent.rate = req->rate * 3;
241
+		else
242
+			req_parent.rate = req->rate << div;
243
+
244
+		if (__clk_determine_rate(parent, &req_parent))
245
+			continue;
246
+
247
+		clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
248
+					     &best_rate, &best_diff, div);
249
+
250
+		if (!best_diff)
251
+			break;
252
+	}
253
+
254
+end:
255
+	pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
256
+		 __func__, best_rate,
257
+		 __clk_get_name((req->best_parent_hw)->clk),
258
+		req->best_parent_rate);
259
+
260
+	if (best_rate < 0)
261
+		return -EINVAL;
262
+
263
+	req->rate = best_rate;
264
+
265
+	return 0;
266
+}
267
+
268
+static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
269
+{
270
+	struct clk_master *master = to_clk_master(hw);
271
+	unsigned long flags;
272
+	u8 index;
273
+
274
+	spin_lock_irqsave(master->lock, flags);
275
+	index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
276
+				     master->parent);
277
+	spin_unlock_irqrestore(master->lock, flags);
278
+
279
+	return index;
280
+}
281
+
282
+static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
283
+{
284
+	struct clk_master *master = to_clk_master(hw);
285
+	unsigned long flags;
286
+
287
+	if (index >= clk_hw_get_num_parents(hw))
288
+		return -EINVAL;
289
+
290
+	spin_lock_irqsave(master->lock, flags);
291
+	master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
292
+	spin_unlock_irqrestore(master->lock, flags);
293
+
294
+	return 0;
295
+}
296
+
297
+static int clk_sama7g5_master_enable(struct clk_hw *hw)
298
+{
299
+	struct clk_master *master = to_clk_master(hw);
300
+	unsigned long flags;
301
+	unsigned int val, cparent;
302
+
303
+	spin_lock_irqsave(master->lock, flags);
304
+
305
+	regmap_write(master->regmap, PMC_MCR, PMC_MCR_ID(master->id));
306
+	regmap_read(master->regmap, PMC_MCR, &val);
307
+	regmap_update_bits(master->regmap, PMC_MCR,
308
+			   PMC_MCR_EN | PMC_MCR_CSS | PMC_MCR_DIV |
309
+			   PMC_MCR_CMD | PMC_MCR_ID_MSK,
310
+			   PMC_MCR_EN | (master->parent << PMC_MCR_CSS_SHIFT) |
311
+			   (master->div << MASTER_DIV_SHIFT) |
312
+			   PMC_MCR_CMD | PMC_MCR_ID(master->id));
313
+
314
+	cparent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
315
+
316
+	/* Wait here only if parent is being changed. */
317
+	while ((cparent != master->parent) && !clk_master_ready(master))
318
+		cpu_relax();
319
+
320
+	spin_unlock_irqrestore(master->lock, flags);
321
+
322
+	return 0;
323
+}
324
+
325
+static void clk_sama7g5_master_disable(struct clk_hw *hw)
326
+{
327
+	struct clk_master *master = to_clk_master(hw);
328
+	unsigned long flags;
329
+
330
+	spin_lock_irqsave(master->lock, flags);
331
+
332
+	regmap_write(master->regmap, PMC_MCR, master->id);
333
+	regmap_update_bits(master->regmap, PMC_MCR,
334
+			   PMC_MCR_EN | PMC_MCR_CMD | PMC_MCR_ID_MSK,
335
+			   PMC_MCR_CMD | PMC_MCR_ID(master->id));
336
+
337
+	spin_unlock_irqrestore(master->lock, flags);
338
+}
339
+
340
+static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
341
+{
342
+	struct clk_master *master = to_clk_master(hw);
343
+	unsigned long flags;
344
+	unsigned int val;
345
+
346
+	spin_lock_irqsave(master->lock, flags);
347
+
348
+	regmap_write(master->regmap, PMC_MCR, master->id);
349
+	regmap_read(master->regmap, PMC_MCR, &val);
350
+
351
+	spin_unlock_irqrestore(master->lock, flags);
352
+
353
+	return !!(val & PMC_MCR_EN);
354
+}
355
+
356
+static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
357
+				       unsigned long parent_rate)
358
+{
359
+	struct clk_master *master = to_clk_master(hw);
360
+	unsigned long div, flags;
361
+
362
+	div = DIV_ROUND_CLOSEST(parent_rate, rate);
363
+	if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))
364
+		return -EINVAL;
365
+
366
+	if (div == 3)
367
+		div = MASTER_PRES_MAX;
368
+	else
369
+		div = ffs(div) - 1;
370
+
371
+	spin_lock_irqsave(master->lock, flags);
372
+	master->div = div;
373
+	spin_unlock_irqrestore(master->lock, flags);
374
+
375
+	return 0;
376
+}
377
+
378
+static const struct clk_ops sama7g5_master_ops = {
379
+	.enable = clk_sama7g5_master_enable,
380
+	.disable = clk_sama7g5_master_disable,
381
+	.is_enabled = clk_sama7g5_master_is_enabled,
382
+	.recalc_rate = clk_sama7g5_master_recalc_rate,
383
+	.determine_rate = clk_sama7g5_master_determine_rate,
384
+	.set_rate = clk_sama7g5_master_set_rate,
385
+	.get_parent = clk_sama7g5_master_get_parent,
386
+	.set_parent = clk_sama7g5_master_set_parent,
387
+};
388
+
389
+struct clk_hw * __init
390
+at91_clk_sama7g5_register_master(struct regmap *regmap,
391
+				 const char *name, int num_parents,
392
+				 const char **parent_names,
393
+				 u32 *mux_table,
394
+				 spinlock_t *lock, u8 id,
395
+				 bool critical, int chg_pid)
396
+{
397
+	struct clk_master *master;
398
+	struct clk_hw *hw;
399
+	struct clk_init_data init;
400
+	unsigned long flags;
401
+	unsigned int val;
402
+	int ret;
403
+
404
+	if (!name || !num_parents || !parent_names || !mux_table ||
405
+	    !lock || id > MASTER_MAX_ID)
406
+		return ERR_PTR(-EINVAL);
407
+
408
+	master = kzalloc(sizeof(*master), GFP_KERNEL);
409
+	if (!master)
410
+		return ERR_PTR(-ENOMEM);
411
+
412
+	init.name = name;
413
+	init.ops = &sama7g5_master_ops;
414
+	init.parent_names = parent_names;
415
+	init.num_parents = num_parents;
416
+	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
417
+	if (chg_pid >= 0)
418
+		init.flags |= CLK_SET_RATE_PARENT;
419
+	if (critical)
420
+		init.flags |= CLK_IS_CRITICAL;
421
+
422
+	master->hw.init = &init;
423
+	master->regmap = regmap;
424
+	master->id = id;
425
+	master->chg_pid = chg_pid;
426
+	master->lock = lock;
427
+	master->mux_table = mux_table;
428
+
429
+	spin_lock_irqsave(master->lock, flags);
430
+	regmap_write(master->regmap, PMC_MCR, master->id);
431
+	regmap_read(master->regmap, PMC_MCR, &val);
432
+	master->parent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
433
+	master->div = (val & PMC_MCR_DIV) >> MASTER_DIV_SHIFT;
434
+	spin_unlock_irqrestore(master->lock, flags);
435
+
436
+	hw = &master->hw;
437
+	ret = clk_hw_register(NULL, &master->hw);
438
+	if (ret) {
439
+		kfree(master);
440
+		hw = ERR_PTR(ret);
441
+	}
442
+
443
+	return hw;
444
+}
445
+
446
+const struct clk_master_layout at91rm9200_master_layout = {
165447 	.mask = 0x31F,
166448 	.pres_shift = 2,
449
+	.offset = AT91_PMC_MCKR,
167450 };
168451 
169
-static const struct clk_master_layout at91sam9x5_master_layout = {
452
+const struct clk_master_layout at91sam9x5_master_layout = {
170453 	.mask = 0x373,
171454 	.pres_shift = 4,
455
+	.offset = AT91_PMC_MCKR,
172456 };
173
-
174
-
175
-static struct clk_master_characteristics * __init
176
-of_at91_clk_master_get_characteristics(struct device_node *np)
177
-{
178
-	struct clk_master_characteristics *characteristics;
179
-
180
-	characteristics = kzalloc(sizeof(*characteristics), GFP_KERNEL);
181
-	if (!characteristics)
182
-		return NULL;
183
-
184
-	if (of_at91_get_clk_range(np, "atmel,clk-output-range", &characteristics->output))
185
-		goto out_free_characteristics;
186
-
187
-	of_property_read_u32_array(np, "atmel,clk-divisors",
188
-				   characteristics->divisors, 4);
189
-
190
-	characteristics->have_div3_pres =
191
-		of_property_read_bool(np, "atmel,master-clk-have-div3-pres");
192
-
193
-	return characteristics;
194
-
195
-out_free_characteristics:
196
-	kfree(characteristics);
197
-	return NULL;
198
-}
199
-
200
-static void __init
201
-of_at91_clk_master_setup(struct device_node *np,
202
-			 const struct clk_master_layout *layout)
203
-{
204
-	struct clk_hw *hw;
205
-	unsigned int num_parents;
206
-	const char *parent_names[MASTER_SOURCE_MAX];
207
-	const char *name = np->name;
208
-	struct clk_master_characteristics *characteristics;
209
-	struct regmap *regmap;
210
-
211
-	num_parents = of_clk_get_parent_count(np);
212
-	if (num_parents == 0 || num_parents > MASTER_SOURCE_MAX)
213
-		return;
214
-
215
-	of_clk_parent_fill(np, parent_names, num_parents);
216
-
217
-	of_property_read_string(np, "clock-output-names", &name);
218
-
219
-	characteristics = of_at91_clk_master_get_characteristics(np);
220
-	if (!characteristics)
221
-		return;
222
-
223
-	regmap = syscon_node_to_regmap(of_get_parent(np));
224
-	if (IS_ERR(regmap))
225
-		return;
226
-
227
-	hw = at91_clk_register_master(regmap, name, num_parents,
228
-				       parent_names, layout,
229
-				       characteristics);
230
-	if (IS_ERR(hw))
231
-		goto out_free_characteristics;
232
-
233
-	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
234
-	return;
235
-
236
-out_free_characteristics:
237
-	kfree(characteristics);
238
-}
239
-
240
-static void __init of_at91rm9200_clk_master_setup(struct device_node *np)
241
-{
242
-	of_at91_clk_master_setup(np, &at91rm9200_master_layout);
243
-}
244
-CLK_OF_DECLARE(at91rm9200_clk_master, "atmel,at91rm9200-clk-master",
245
-	       of_at91rm9200_clk_master_setup);
246
-
247
-static void __init of_at91sam9x5_clk_master_setup(struct device_node *np)
248
-{
249
-	of_at91_clk_master_setup(np, &at91sam9x5_master_layout);
250
-}
251
-CLK_OF_DECLARE(at91sam9x5_clk_master, "atmel,at91sam9x5-clk-master",
252
-	       of_at91sam9x5_clk_master_setup);