1 // SPDX-License-Identifier: GPL-2.0
3 * Copyright (c) 2017, Linaro Limited
4 * Author: Georgi Djakov <georgi.djakov@linaro.org>
7 #include <linux/bitops.h>
8 #include <linux/delay.h>
9 #include <linux/kernel.h>
10 #include <linux/regmap.h>
12 #include "clk-regmap-mux-div.h"
15 #define CMD_RCGR_UPDATE BIT(0)
16 #define CMD_RCGR_DIRTY_CFG BIT(4)
17 #define CMD_RCGR_ROOT_OFF BIT(31)
20 #define to_clk_regmap_mux_div(_hw) \
21 container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr)
23 int mux_div_set_src_div(struct clk_regmap_mux_div
*md
, u32 src
, u32 div
)
27 const char *name
= clk_hw_get_name(&md
->clkr
.hw
);
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
);
33 ret
= regmap_update_bits(md
->clkr
.regmap
, CFG_RCGR
+ md
->reg_offset
,
38 ret
= regmap_update_bits(md
->clkr
.regmap
, CMD_RCGR
+ md
->reg_offset
,
39 CMD_RCGR_UPDATE
, CMD_RCGR_UPDATE
);
43 /* Wait for update to take effect */
44 for (count
= 500; count
> 0; count
--) {
45 ret
= regmap_read(md
->clkr
.regmap
, CMD_RCGR
+ md
->reg_offset
,
49 if (!(val
& CMD_RCGR_UPDATE
))
54 pr_err("%s: RCG did not update its configuration", name
);
57 EXPORT_SYMBOL_GPL(mux_div_set_src_div
);
59 static void mux_div_get_src_div(struct clk_regmap_mux_div
*md
, u32
*src
,
63 const char *name
= clk_hw_get_name(&md
->clkr
.hw
);
65 regmap_read(md
->clkr
.regmap
, CMD_RCGR
+ md
->reg_offset
, &val
);
67 if (val
& CMD_RCGR_DIRTY_CFG
) {
68 pr_err("%s: RCG configuration is pending\n", name
);
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;
77 d
= (val
>> md
->hid_shift
);
78 d
&= BIT(md
->hid_width
) - 1;
82 static inline bool is_better_rate(unsigned long req
, unsigned long best
,
85 return (req
<= new && new < best
) || (best
< req
&& best
< new);
88 static int mux_div_determine_rate(struct clk_hw
*hw
,
89 struct clk_rate_request
*req
)
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
;
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
);
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
);
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
;
113 if (actual_rate
< req_rate
|| best_rate
<= req_rate
)
124 static int __mux_div_set_rate_and_parent(struct clk_hw
*hw
, unsigned long rate
,
125 unsigned long prate
, u32 src
)
127 struct clk_regmap_mux_div
*md
= to_clk_regmap_mux_div(hw
);
129 u32 div
, max_div
, best_src
= 0, best_div
= 0;
131 unsigned long actual_rate
, best_rate
= 0;
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
);
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
);
143 if (is_better_rate(rate
, best_rate
, actual_rate
)) {
144 best_rate
= actual_rate
;
145 best_src
= md
->parent_map
[i
];
149 if (actual_rate
< rate
|| best_rate
<= rate
)
154 ret
= mux_div_set_src_div(md
, best_src
, best_div
);
163 static u8
mux_div_get_parent(struct clk_hw
*hw
)
165 struct clk_regmap_mux_div
*md
= to_clk_regmap_mux_div(hw
);
166 const char *name
= clk_hw_get_name(hw
);
169 mux_div_get_src_div(md
, &src
, &div
);
171 for (i
= 0; i
< clk_hw_get_num_parents(hw
); i
++)
172 if (src
== md
->parent_map
[i
])
175 pr_err("%s: Can't find parent with src %d\n", name
, src
);
179 static int mux_div_set_parent(struct clk_hw
*hw
, u8 index
)
181 struct clk_regmap_mux_div
*md
= to_clk_regmap_mux_div(hw
);
183 return mux_div_set_src_div(md
, md
->parent_map
[index
], md
->div
);
186 static int mux_div_set_rate(struct clk_hw
*hw
,
187 unsigned long rate
, unsigned long prate
)
189 struct clk_regmap_mux_div
*md
= to_clk_regmap_mux_div(hw
);
191 return __mux_div_set_rate_and_parent(hw
, rate
, prate
, md
->src
);
194 static int mux_div_set_rate_and_parent(struct clk_hw
*hw
, unsigned long rate
,
195 unsigned long prate
, u8 index
)
197 struct clk_regmap_mux_div
*md
= to_clk_regmap_mux_div(hw
);
199 return __mux_div_set_rate_and_parent(hw
, rate
, prate
,
200 md
->parent_map
[index
]);
203 static unsigned long mux_div_recalc_rate(struct clk_hw
*hw
, unsigned long prate
)
205 struct clk_regmap_mux_div
*md
= to_clk_regmap_mux_div(hw
);
207 int i
, num_parents
= clk_hw_get_num_parents(hw
);
208 const char *name
= clk_hw_get_name(hw
);
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
);
216 return mult_frac(parent_rate
, 2, div
+ 1);
219 pr_err("%s: Can't find parent %d\n", name
, src
);
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
,
231 EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops
);