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 
mux_div_set_src_div(struct clk_regmap_mux_div * md,u32 src,u32 div)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(&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 	/* 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,
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 }
57 EXPORT_SYMBOL_GPL(mux_div_set_src_div);
58 
mux_div_get_src_div(struct clk_regmap_mux_div * md,u32 * src,u32 * div)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(&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 
is_better_rate(unsigned long req,unsigned long best,unsigned long new)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 
mux_div_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)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, 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 
__mux_div_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long prate,u32 src)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, 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 
mux_div_get_parent(struct clk_hw * hw)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, &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 
mux_div_set_parent(struct clk_hw * hw,u8 index)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 
mux_div_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long prate)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, md->src);
192 }
193 
mux_div_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long prate,u8 index)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 					     md->parent_map[index]);
201 }
202 
mux_div_recalc_rate(struct clk_hw * hw,unsigned long prate)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, &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 
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