1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (c) 2017, Linaro Limited |
4 | * Author: Georgi Djakov <georgi.djakov@linaro.org> |
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 | |
23 | int 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(hw: &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(map: md->clkr.regmap, CFG_RCGR + md->reg_offset, |
34 | mask, val); |
35 | if (ret) |
36 | return ret; |
37 | |
38 | ret = regmap_update_bits(map: md->clkr.regmap, CMD_RCGR + md->reg_offset, |
39 | CMD_RCGR_UPDATE, CMD_RCGR_UPDATE); |
40 | if (ret) |
41 | return ret; |
42 | |
43 | /* Wait for update to take effect */ |
44 | for (count = 500; count > 0; count--) { |
45 | ret = regmap_read(map: md->clkr.regmap, CMD_RCGR + md->reg_offset, |
46 | val: &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 | } |
57 | EXPORT_SYMBOL_GPL(mux_div_set_src_div); |
58 | |
59 | static 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(hw: &md->clkr.hw); |
64 | |
65 | regmap_read(map: md->clkr.regmap, CMD_RCGR + md->reg_offset, val: &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(map: md->clkr.regmap, CFG_RCGR + md->reg_offset, val: &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 | |
82 | static 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 | |
88 | static 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, index: i); |
98 | unsigned long parent_rate = clk_hw_get_rate(hw: 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(hw: parent, rate: parent_rate); |
104 | actual_rate = mult_frac(parent_rate, 2, div); |
105 | |
106 | if (is_better_rate(req: req_rate, best: best_rate, new: 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 | |
124 | static 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, index: i); |
135 | unsigned long parent_rate = clk_hw_get_rate(hw: 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(hw: parent, rate: parent_rate); |
141 | actual_rate = mult_frac(parent_rate, 2, div); |
142 | |
143 | if (is_better_rate(req: rate, best: best_rate, new: 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 | |
163 | static 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: &src, div: &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 | |
179 | static 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 | |
186 | static 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, src: md->src); |
192 | } |
193 | |
194 | static 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 | src: md->parent_map[index]); |
201 | } |
202 | |
203 | static 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: &src, div: &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, index: i); |
214 | unsigned long parent_rate = clk_hw_get_rate(hw: 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 | |
223 | const 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 | }; |
231 | EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops); |
232 | |