1
2
3
4
5
6
7#include <linux/bitops.h>
8#include <linux/delay.h>
9#include <linux/kernel.h>
10#include <linux/regmap.h>
11
12#include "clk-regmap-mux-div.h"
13
14#define CMD_RCGR 0x0
15#define CMD_RCGR_UPDATE BIT(0)
16#define CMD_RCGR_DIRTY_CFG BIT(4)
17#define CMD_RCGR_ROOT_OFF BIT(31)
18#define CFG_RCGR 0x4
19
20#define to_clk_regmap_mux_div(_hw) \
21 container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr)
22
23int mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div)
24{
25 int ret, count;
26 u32 val, mask;
27 const char *name = clk_hw_get_name(&md->clkr.hw);
28
29 val = (div << md->hid_shift) | (src << md->src_shift);
30 mask = ((BIT(md->hid_width) - 1) << md->hid_shift) |
31 ((BIT(md->src_width) - 1) << md->src_shift);
32
33 ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset,
34 mask, val);
35 if (ret)
36 return ret;
37
38 ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset,
39 CMD_RCGR_UPDATE, CMD_RCGR_UPDATE);
40 if (ret)
41 return ret;
42
43
44 for (count = 500; count > 0; count--) {
45 ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset,
46 &val);
47 if (ret)
48 return ret;
49 if (!(val & CMD_RCGR_UPDATE))
50 return 0;
51 udelay(1);
52 }
53
54 pr_err("%s: RCG did not update its configuration", name);
55 return -EBUSY;
56}
57EXPORT_SYMBOL_GPL(mux_div_set_src_div);
58
59static void mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src,
60 u32 *div)
61{
62 u32 val, d, s;
63 const char *name = clk_hw_get_name(&md->clkr.hw);
64
65 regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val);
66
67 if (val & CMD_RCGR_DIRTY_CFG) {
68 pr_err("%s: RCG configuration is pending\n", name);
69 return;
70 }
71
72 regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val);
73 s = (val >> md->src_shift);
74 s &= BIT(md->src_width) - 1;
75 *src = s;
76
77 d = (val >> md->hid_shift);
78 d &= BIT(md->hid_width) - 1;
79 *div = d;
80}
81
82static inline bool is_better_rate(unsigned long req, unsigned long best,
83 unsigned long new)
84{
85 return (req <= new && new < best) || (best < req && best < new);
86}
87
88static int mux_div_determine_rate(struct clk_hw *hw,
89 struct clk_rate_request *req)
90{
91 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
92 unsigned int i, div, max_div;
93 unsigned long actual_rate, best_rate = 0;
94 unsigned long req_rate = req->rate;
95
96 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
97 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
98 unsigned long parent_rate = clk_hw_get_rate(parent);
99
100 max_div = BIT(md->hid_width) - 1;
101 for (div = 1; div < max_div; div++) {
102 parent_rate = mult_frac(req_rate, div, 2);
103 parent_rate = clk_hw_round_rate(parent, parent_rate);
104 actual_rate = mult_frac(parent_rate, 2, div);
105
106 if (is_better_rate(req_rate, best_rate, actual_rate)) {
107 best_rate = actual_rate;
108 req->rate = best_rate;
109 req->best_parent_rate = parent_rate;
110 req->best_parent_hw = parent;
111 }
112
113 if (actual_rate < req_rate || best_rate <= req_rate)
114 break;
115 }
116 }
117
118 if (!best_rate)
119 return -EINVAL;
120
121 return 0;
122}
123
124static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
125 unsigned long prate, u32 src)
126{
127 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
128 int ret;
129 u32 div, max_div, best_src = 0, best_div = 0;
130 unsigned int i;
131 unsigned long actual_rate, best_rate = 0;
132
133 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
134 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
135 unsigned long parent_rate = clk_hw_get_rate(parent);
136
137 max_div = BIT(md->hid_width) - 1;
138 for (div = 1; div < max_div; div++) {
139 parent_rate = mult_frac(rate, div, 2);
140 parent_rate = clk_hw_round_rate(parent, parent_rate);
141 actual_rate = mult_frac(parent_rate, 2, div);
142
143 if (is_better_rate(rate, best_rate, actual_rate)) {
144 best_rate = actual_rate;
145 best_src = md->parent_map[i];
146 best_div = div - 1;
147 }
148
149 if (actual_rate < rate || best_rate <= rate)
150 break;
151 }
152 }
153
154 ret = mux_div_set_src_div(md, best_src, best_div);
155 if (!ret) {
156 md->div = best_div;
157 md->src = best_src;
158 }
159
160 return ret;
161}
162
163static u8 mux_div_get_parent(struct clk_hw *hw)
164{
165 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
166 const char *name = clk_hw_get_name(hw);
167 u32 i, div, src = 0;
168
169 mux_div_get_src_div(md, &src, &div);
170
171 for (i = 0; i < clk_hw_get_num_parents(hw); i++)
172 if (src == md->parent_map[i])
173 return i;
174
175 pr_err("%s: Can't find parent with src %d\n", name, src);
176 return 0;
177}
178
179static int mux_div_set_parent(struct clk_hw *hw, u8 index)
180{
181 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
182
183 return mux_div_set_src_div(md, md->parent_map[index], md->div);
184}
185
186static int mux_div_set_rate(struct clk_hw *hw,
187 unsigned long rate, unsigned long prate)
188{
189 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
190
191 return __mux_div_set_rate_and_parent(hw, rate, prate, md->src);
192}
193
194static int mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
195 unsigned long prate, u8 index)
196{
197 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
198
199 return __mux_div_set_rate_and_parent(hw, rate, prate,
200 md->parent_map[index]);
201}
202
203static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate)
204{
205 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
206 u32 div, src;
207 int i, num_parents = clk_hw_get_num_parents(hw);
208 const char *name = clk_hw_get_name(hw);
209
210 mux_div_get_src_div(md, &src, &div);
211 for (i = 0; i < num_parents; i++)
212 if (src == md->parent_map[i]) {
213 struct clk_hw *p = clk_hw_get_parent_by_index(hw, i);
214 unsigned long parent_rate = clk_hw_get_rate(p);
215
216 return mult_frac(parent_rate, 2, div + 1);
217 }
218
219 pr_err("%s: Can't find parent %d\n", name, src);
220 return 0;
221}
222
223const struct clk_ops clk_regmap_mux_div_ops = {
224 .get_parent = mux_div_get_parent,
225 .set_parent = mux_div_set_parent,
226 .set_rate = mux_div_set_rate,
227 .set_rate_and_parent = mux_div_set_rate_and_parent,
228 .determine_rate = mux_div_determine_rate,
229 .recalc_rate = mux_div_recalc_rate,
230};
231EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops);
232
