1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
4  */
5 
6 #include <linux/bitops.h>
7 #include <linux/clk-provider.h>
8 #include <linux/err.h>
9 #include <linux/export.h>
10 #include <linux/io.h>
11 #include <linux/kernel.h>
12 #include <linux/of.h>
13 #include <linux/slab.h>
14 
clk_mult_readl(struct clk_multiplier * mult)15 static inline u32 clk_mult_readl(struct clk_multiplier *mult)
16 {
17 	if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
18 		return ioread32be(mult->reg);
19 
20 	return readl(mult->reg);
21 }
22 
clk_mult_writel(struct clk_multiplier * mult,u32 val)23 static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
24 {
25 	if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
26 		iowrite32be(val, mult->reg);
27 	else
28 		writel(val, mult->reg);
29 }
30 
__get_mult(struct clk_multiplier * mult,unsigned long rate,unsigned long parent_rate)31 static unsigned long __get_mult(struct clk_multiplier *mult,
32 				unsigned long rate,
33 				unsigned long parent_rate)
34 {
35 	if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
36 		return DIV_ROUND_CLOSEST(rate, parent_rate);
37 
38 	return rate / parent_rate;
39 }
40 
clk_multiplier_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)41 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
42 						unsigned long parent_rate)
43 {
44 	struct clk_multiplier *mult = to_clk_multiplier(hw);
45 	unsigned long val;
46 
47 	val = clk_mult_readl(mult) >> mult->shift;
48 	val &= GENMASK(mult->width - 1, 0);
49 
50 	if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
51 		val = 1;
52 
53 	return parent_rate * val;
54 }
55 
__is_best_rate(unsigned long rate,unsigned long new,unsigned long best,unsigned long flags)56 static bool __is_best_rate(unsigned long rate, unsigned long new,
57 			   unsigned long best, unsigned long flags)
58 {
59 	if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
60 		return abs(rate - new) < abs(rate - best);
61 
62 	return new >= rate && new < best;
63 }
64 
__bestmult(struct clk_hw * hw,unsigned long rate,unsigned long * best_parent_rate,u8 width,unsigned long flags)65 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
66 				unsigned long *best_parent_rate,
67 				u8 width, unsigned long flags)
68 {
69 	struct clk_multiplier *mult = to_clk_multiplier(hw);
70 	unsigned long orig_parent_rate = *best_parent_rate;
71 	unsigned long parent_rate, current_rate, best_rate = ~0;
72 	unsigned int i, bestmult = 0;
73 	unsigned int maxmult = (1 << width) - 1;
74 
75 	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
76 		bestmult = rate / orig_parent_rate;
77 
78 		/* Make sure we don't end up with a 0 multiplier */
79 		if ((bestmult == 0) &&
80 		    !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
81 			bestmult = 1;
82 
83 		/* Make sure we don't overflow the multiplier */
84 		if (bestmult > maxmult)
85 			bestmult = maxmult;
86 
87 		return bestmult;
88 	}
89 
90 	for (i = 1; i < maxmult; i++) {
91 		if (rate == orig_parent_rate * i) {
92 			/*
93 			 * This is the best case for us if we have a
94 			 * perfect match without changing the parent
95 			 * rate.
96 			 */
97 			*best_parent_rate = orig_parent_rate;
98 			return i;
99 		}
100 
101 		parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
102 						rate / i);
103 		current_rate = parent_rate * i;
104 
105 		if (__is_best_rate(rate, current_rate, best_rate, flags)) {
106 			bestmult = i;
107 			best_rate = current_rate;
108 			*best_parent_rate = parent_rate;
109 		}
110 	}
111 
112 	return bestmult;
113 }
114 
clk_multiplier_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)115 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
116 				  unsigned long *parent_rate)
117 {
118 	struct clk_multiplier *mult = to_clk_multiplier(hw);
119 	unsigned long factor = __bestmult(hw, rate, parent_rate,
120 					  mult->width, mult->flags);
121 
122 	return *parent_rate * factor;
123 }
124 
clk_multiplier_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)125 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
126 			       unsigned long parent_rate)
127 {
128 	struct clk_multiplier *mult = to_clk_multiplier(hw);
129 	unsigned long factor = __get_mult(mult, rate, parent_rate);
130 	unsigned long flags = 0;
131 	unsigned long val;
132 
133 	if (mult->lock)
134 		spin_lock_irqsave(mult->lock, flags);
135 	else
136 		__acquire(mult->lock);
137 
138 	val = clk_mult_readl(mult);
139 	val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
140 	val |= factor << mult->shift;
141 	clk_mult_writel(mult, val);
142 
143 	if (mult->lock)
144 		spin_unlock_irqrestore(mult->lock, flags);
145 	else
146 		__release(mult->lock);
147 
148 	return 0;
149 }
150 
151 const struct clk_ops clk_multiplier_ops = {
152 	.recalc_rate	= clk_multiplier_recalc_rate,
153 	.round_rate	= clk_multiplier_round_rate,
154 	.set_rate	= clk_multiplier_set_rate,
155 };
156 EXPORT_SYMBOL_GPL(clk_multiplier_ops);
157