1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3  * Copyright (c) 2021, Bootlin
4  */
5 
6 #include <drivers/clk.h>
7 #include <kernel/boot.h>
8 #include <kernel/panic.h>
9 #include <kernel/spinlock.h>
10 #include <libfdt.h>
11 #include <malloc.h>
12 #include <stddef.h>
13 
14 /* Global clock tree lock */
15 static unsigned int clk_lock = SPINLOCK_UNLOCK;
16 
clk_alloc(const char * name,const struct clk_ops * ops,struct clk ** parent_clks,size_t parent_count)17 struct clk *clk_alloc(const char *name, const struct clk_ops *ops,
18 		      struct clk **parent_clks, size_t parent_count)
19 {
20 	struct clk *clk = NULL;
21 	size_t parent = 0;
22 
23 	clk = calloc(1, sizeof(*clk) + parent_count * sizeof(clk));
24 	if (!clk)
25 		return NULL;
26 
27 	clk->num_parents = parent_count;
28 	for (parent = 0; parent < parent_count; parent++)
29 		clk->parents[parent] = parent_clks[parent];
30 
31 	clk->name = name;
32 	clk->ops = ops;
33 	refcount_set(&clk->enabled_count, 0);
34 
35 	return clk;
36 }
37 
clk_free(struct clk * clk)38 void clk_free(struct clk *clk)
39 {
40 	free(clk);
41 }
42 
clk_check(struct clk * clk)43 static bool __maybe_unused clk_check(struct clk *clk)
44 {
45 	if (!clk->ops)
46 		return false;
47 
48 	if (clk->ops->set_parent && !clk->ops->get_parent)
49 		return false;
50 
51 	if (clk->num_parents > 1 && !clk->ops->get_parent)
52 		return false;
53 
54 	return true;
55 }
56 
clk_compute_rate_no_lock(struct clk * clk)57 static void clk_compute_rate_no_lock(struct clk *clk)
58 {
59 	unsigned long parent_rate = 0;
60 
61 	if (clk->parent)
62 		parent_rate = clk->parent->rate;
63 
64 	if (clk->ops->get_rate)
65 		clk->rate = clk->ops->get_rate(clk, parent_rate);
66 	else
67 		clk->rate = parent_rate;
68 }
69 
clk_get_parent_by_index(struct clk * clk,size_t pidx)70 struct clk *clk_get_parent_by_index(struct clk *clk, size_t pidx)
71 {
72 	if (pidx >= clk->num_parents)
73 		return NULL;
74 
75 	return clk->parents[pidx];
76 }
77 
clk_init_parent(struct clk * clk)78 static void clk_init_parent(struct clk *clk)
79 {
80 	size_t pidx = 0;
81 
82 	switch (clk->num_parents) {
83 	case 0:
84 		break;
85 	case 1:
86 		clk->parent = clk->parents[0];
87 		break;
88 	default:
89 		pidx = clk->ops->get_parent(clk);
90 		assert(pidx < clk->num_parents);
91 
92 		clk->parent = clk->parents[pidx];
93 		break;
94 	}
95 }
96 
clk_register(struct clk * clk)97 TEE_Result clk_register(struct clk *clk)
98 {
99 	assert(clk_check(clk));
100 
101 	clk_init_parent(clk);
102 	clk_compute_rate_no_lock(clk);
103 
104 	DMSG("Registered clock %s, freq %lu", clk->name, clk_get_rate(clk));
105 
106 	return TEE_SUCCESS;
107 }
108 
clk_is_enabled_no_lock(struct clk * clk)109 static bool clk_is_enabled_no_lock(struct clk *clk)
110 {
111 	return refcount_val(&clk->enabled_count) != 0;
112 }
113 
clk_is_enabled(struct clk * clk)114 bool clk_is_enabled(struct clk *clk)
115 {
116 	return clk_is_enabled_no_lock(clk);
117 }
118 
clk_disable_no_lock(struct clk * clk)119 static void clk_disable_no_lock(struct clk *clk)
120 {
121 	struct clk *parent = NULL;
122 
123 	if (!refcount_dec(&clk->enabled_count))
124 		return;
125 
126 	if (clk->ops->disable)
127 		clk->ops->disable(clk);
128 
129 	parent = clk_get_parent(clk);
130 	if (parent)
131 		clk_disable_no_lock(parent);
132 }
133 
clk_enable_no_lock(struct clk * clk)134 static TEE_Result clk_enable_no_lock(struct clk *clk)
135 {
136 	TEE_Result res = TEE_ERROR_GENERIC;
137 	struct clk *parent = NULL;
138 
139 	if (refcount_inc(&clk->enabled_count))
140 		return TEE_SUCCESS;
141 
142 	parent = clk_get_parent(clk);
143 	if (parent) {
144 		res = clk_enable_no_lock(parent);
145 		if (res)
146 			return res;
147 	}
148 
149 	if (clk->ops->enable) {
150 		res = clk->ops->enable(clk);
151 		if (res) {
152 			if (parent)
153 				clk_disable_no_lock(parent);
154 
155 			return res;
156 		}
157 	}
158 
159 	refcount_set(&clk->enabled_count, 1);
160 
161 	return TEE_SUCCESS;
162 }
163 
clk_enable(struct clk * clk)164 TEE_Result clk_enable(struct clk *clk)
165 {
166 	uint32_t exceptions = 0;
167 	TEE_Result res = TEE_ERROR_GENERIC;
168 
169 	exceptions = cpu_spin_lock_xsave(&clk_lock);
170 	res = clk_enable_no_lock(clk);
171 	cpu_spin_unlock_xrestore(&clk_lock, exceptions);
172 
173 	return res;
174 }
175 
clk_disable(struct clk * clk)176 void clk_disable(struct clk *clk)
177 {
178 	uint32_t exceptions = 0;
179 
180 	exceptions = cpu_spin_lock_xsave(&clk_lock);
181 	clk_disable_no_lock(clk);
182 	cpu_spin_unlock_xrestore(&clk_lock, exceptions);
183 }
184 
clk_get_rate(struct clk * clk)185 unsigned long clk_get_rate(struct clk *clk)
186 {
187 	return clk->rate;
188 }
189 
clk_set_rate_no_lock(struct clk * clk,unsigned long rate)190 static TEE_Result clk_set_rate_no_lock(struct clk *clk, unsigned long rate)
191 {
192 	TEE_Result res = TEE_ERROR_GENERIC;
193 	unsigned long parent_rate = 0;
194 
195 	if (clk->parent)
196 		parent_rate = clk_get_rate(clk->parent);
197 
198 	res = clk->ops->set_rate(clk, rate, parent_rate);
199 	if (res)
200 		return res;
201 
202 	clk_compute_rate_no_lock(clk);
203 
204 	return TEE_SUCCESS;
205 }
206 
clk_set_rate(struct clk * clk,unsigned long rate)207 TEE_Result clk_set_rate(struct clk *clk, unsigned long rate)
208 {
209 	uint32_t exceptions = 0;
210 	TEE_Result res = TEE_ERROR_GENERIC;
211 
212 	if (!clk->ops->set_rate)
213 		return TEE_ERROR_NOT_SUPPORTED;
214 
215 	exceptions =  cpu_spin_lock_xsave(&clk_lock);
216 
217 	if (clk->flags & CLK_SET_RATE_GATE && clk_is_enabled_no_lock(clk))
218 		res = TEE_ERROR_BAD_STATE;
219 	else
220 		res = clk_set_rate_no_lock(clk, rate);
221 
222 	cpu_spin_unlock_xrestore(&clk_lock, exceptions);
223 
224 	return res;
225 }
226 
clk_get_parent(struct clk * clk)227 struct clk *clk_get_parent(struct clk *clk)
228 {
229 	return clk->parent;
230 }
231 
clk_get_parent_idx(struct clk * clk,struct clk * parent,size_t * pidx)232 static TEE_Result clk_get_parent_idx(struct clk *clk, struct clk *parent,
233 				     size_t *pidx)
234 {
235 	size_t i = 0;
236 
237 	for (i = 0; i < clk_get_num_parents(clk); i++) {
238 		if (clk_get_parent_by_index(clk, i) == parent) {
239 			*pidx = i;
240 			return TEE_SUCCESS;
241 		}
242 	}
243 	EMSG("Clock %s is not a parent of clock %s", parent->name, clk->name);
244 
245 	return TEE_ERROR_BAD_PARAMETERS;
246 }
247 
clk_set_parent_no_lock(struct clk * clk,struct clk * parent,size_t pidx)248 static TEE_Result clk_set_parent_no_lock(struct clk *clk, struct clk *parent,
249 					 size_t pidx)
250 {
251 	TEE_Result res = TEE_ERROR_GENERIC;
252 	bool was_enabled = false;
253 
254 	/* Requested parent is already the one set */
255 	if (clk->parent == parent)
256 		return TEE_SUCCESS;
257 
258 	was_enabled = clk_is_enabled_no_lock(clk);
259 	/* Call is needed to decrement refcount on current parent tree */
260 	if (was_enabled)
261 		clk_disable_no_lock(clk);
262 
263 	res = clk->ops->set_parent(clk, pidx);
264 	if (res)
265 		goto out;
266 
267 	clk->parent = parent;
268 
269 	/* The parent changed and the rate might also have changed */
270 	clk_compute_rate_no_lock(clk);
271 
272 out:
273 	/* Call is needed to increment refcount on the new parent tree */
274 	if (was_enabled) {
275 		res = clk_enable_no_lock(clk);
276 		if (res)
277 			panic("Failed to re-enable clock after setting parent");
278 	}
279 
280 	return res;
281 }
282 
clk_set_parent(struct clk * clk,struct clk * parent)283 TEE_Result clk_set_parent(struct clk *clk, struct clk *parent)
284 {
285 	size_t pidx = 0;
286 	uint32_t exceptions = 0;
287 	TEE_Result res = TEE_ERROR_GENERIC;
288 
289 	if (clk_get_parent_idx(clk, parent, &pidx) || !clk->ops->set_parent)
290 		return TEE_ERROR_BAD_PARAMETERS;
291 
292 	exceptions = cpu_spin_lock_xsave(&clk_lock);
293 	if (clk->flags & CLK_SET_PARENT_GATE && clk_is_enabled_no_lock(clk)) {
294 		res = TEE_ERROR_BAD_STATE;
295 		goto out;
296 	}
297 
298 	res = clk_set_parent_no_lock(clk, parent, pidx);
299 out:
300 	cpu_spin_unlock_xrestore(&clk_lock, exceptions);
301 
302 	return res;
303 }
304 
clk_get_rates_array(struct clk * clk,size_t start_index,unsigned long * rates,size_t * nb_elts)305 TEE_Result clk_get_rates_array(struct clk *clk, size_t start_index,
306 			       unsigned long *rates, size_t *nb_elts)
307 {
308 	if (!clk->ops->get_rates_array)
309 		return TEE_ERROR_NOT_SUPPORTED;
310 
311 	return clk->ops->get_rates_array(clk, start_index, rates, nb_elts);
312 }
313