1
2
3
4
5
6
7
8
9
10
11#include <linux/clk-provider.h>
12#include <linux/regmap.h>
13#include <linux/slab.h>
14
15#include "owl-factor.h"
16
17static unsigned int _get_table_maxval(const struct clk_factor_table *table)
18{
19 unsigned int maxval = 0;
20 const struct clk_factor_table *clkt;
21
22 for (clkt = table; clkt->div; clkt++)
23 if (clkt->val > maxval)
24 maxval = clkt->val;
25 return maxval;
26}
27
28static int _get_table_div_mul(const struct clk_factor_table *table,
29 unsigned int val, unsigned int *mul, unsigned int *div)
30{
31 const struct clk_factor_table *clkt;
32
33 for (clkt = table; clkt->div; clkt++) {
34 if (clkt->val == val) {
35 *mul = clkt->mul;
36 *div = clkt->div;
37 return 1;
38 }
39 }
40
41 return 0;
42}
43
44static unsigned int _get_table_val(const struct clk_factor_table *table,
45 unsigned long rate, unsigned long parent_rate)
46{
47 const struct clk_factor_table *clkt;
48 int val = -1;
49 u64 calc_rate;
50
51 for (clkt = table; clkt->div; clkt++) {
52 calc_rate = parent_rate * clkt->mul;
53 do_div(calc_rate, clkt->div);
54
55 if ((unsigned long)calc_rate <= rate) {
56 val = clkt->val;
57 break;
58 }
59 }
60
61 if (val == -1)
62 val = _get_table_maxval(table);
63
64 return val;
65}
66
67static int owl_clk_val_best(const struct owl_factor_hw *factor_hw,
68 struct clk_hw *hw, unsigned long rate,
69 unsigned long *best_parent_rate)
70{
71 const struct clk_factor_table *clkt = factor_hw->table;
72 unsigned long parent_rate, try_parent_rate, best = 0, cur_rate;
73 unsigned long parent_rate_saved = *best_parent_rate;
74 int bestval = 0;
75
76 if (!rate)
77 rate = 1;
78
79 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
80 parent_rate = *best_parent_rate;
81 bestval = _get_table_val(clkt, rate, parent_rate);
82 return bestval;
83 }
84
85 for (clkt = factor_hw->table; clkt->div; clkt++) {
86 try_parent_rate = rate * clkt->div / clkt->mul;
87
88 if (try_parent_rate == parent_rate_saved) {
89 pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n",
90 __func__, clkt->val, clkt->mul, clkt->div,
91 try_parent_rate);
92
93
94
95
96
97 *best_parent_rate = parent_rate_saved;
98 return clkt->val;
99 }
100
101 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
102 try_parent_rate);
103 cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul;
104 if (cur_rate <= rate && cur_rate > best) {
105 bestval = clkt->val;
106 best = cur_rate;
107 *best_parent_rate = parent_rate;
108 }
109 }
110
111 if (!bestval) {
112 bestval = _get_table_maxval(clkt);
113 *best_parent_rate = clk_hw_round_rate(
114 clk_hw_get_parent(hw), 1);
115 }
116
117 return bestval;
118}
119
120long owl_factor_helper_round_rate(struct owl_clk_common *common,
121 const struct owl_factor_hw *factor_hw,
122 unsigned long rate,
123 unsigned long *parent_rate)
124{
125 const struct clk_factor_table *clkt = factor_hw->table;
126 unsigned int val, mul = 0, div = 1;
127
128 val = owl_clk_val_best(factor_hw, &common->hw, rate, parent_rate);
129 _get_table_div_mul(clkt, val, &mul, &div);
130
131 return *parent_rate * mul / div;
132}
133
134static long owl_factor_round_rate(struct clk_hw *hw, unsigned long rate,
135 unsigned long *parent_rate)
136{
137 struct owl_factor *factor = hw_to_owl_factor(hw);
138 struct owl_factor_hw *factor_hw = &factor->factor_hw;
139
140 return owl_factor_helper_round_rate(&factor->common, factor_hw,
141 rate, parent_rate);
142}
143
144unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common,
145 const struct owl_factor_hw *factor_hw,
146 unsigned long parent_rate)
147{
148 const struct clk_factor_table *clkt = factor_hw->table;
149 unsigned long long int rate;
150 u32 reg, val, mul, div;
151
152 div = 0;
153 mul = 0;
154
155 regmap_read(common->regmap, factor_hw->reg, ®);
156
157 val = reg >> factor_hw->shift;
158 val &= div_mask(factor_hw);
159
160 _get_table_div_mul(clkt, val, &mul, &div);
161 if (!div) {
162 WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO),
163 "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
164 __clk_get_name(common->hw.clk));
165 return parent_rate;
166 }
167
168 rate = (unsigned long long int)parent_rate * mul;
169 do_div(rate, div);
170
171 return rate;
172}
173
174static unsigned long owl_factor_recalc_rate(struct clk_hw *hw,
175 unsigned long parent_rate)
176{
177 struct owl_factor *factor = hw_to_owl_factor(hw);
178 struct owl_factor_hw *factor_hw = &factor->factor_hw;
179 struct owl_clk_common *common = &factor->common;
180
181 return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate);
182}
183
184int owl_factor_helper_set_rate(const struct owl_clk_common *common,
185 const struct owl_factor_hw *factor_hw,
186 unsigned long rate,
187 unsigned long parent_rate)
188{
189 u32 val, reg;
190
191 val = _get_table_val(factor_hw->table, rate, parent_rate);
192
193 if (val > div_mask(factor_hw))
194 val = div_mask(factor_hw);
195
196 regmap_read(common->regmap, factor_hw->reg, ®);
197
198 reg &= ~(div_mask(factor_hw) << factor_hw->shift);
199 reg |= val << factor_hw->shift;
200
201 regmap_write(common->regmap, factor_hw->reg, reg);
202
203 return 0;
204}
205
206static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate,
207 unsigned long parent_rate)
208{
209 struct owl_factor *factor = hw_to_owl_factor(hw);
210 struct owl_factor_hw *factor_hw = &factor->factor_hw;
211 struct owl_clk_common *common = &factor->common;
212
213 return owl_factor_helper_set_rate(common, factor_hw,
214 rate, parent_rate);
215}
216
217const struct clk_ops owl_factor_ops = {
218 .round_rate = owl_factor_round_rate,
219 .recalc_rate = owl_factor_recalc_rate,
220 .set_rate = owl_factor_set_rate,
221};
222
