1 /*
2 * Arm SCP/MCP Software
3 * Copyright (c) 2017-2021, Arm Limited and Contributors. All rights reserved.
4 *
5 * SPDX-License-Identifier: BSD-3-Clause
6 */
7
8 #include <mod_clock.h>
9 #include <mod_css_clock.h>
10 #include <mod_power_domain.h>
11
12 #include <fwk_id.h>
13 #include <fwk_mm.h>
14 #include <fwk_module.h>
15 #include <fwk_status.h>
16
17 #include <stdint.h>
18 #include <stdlib.h>
19
20 /* Device context */
21 struct css_clock_dev_ctx {
22 bool initialized;
23 uint64_t current_rate;
24 struct mod_clock_drv_api *pll_api;
25 struct mod_css_clock_direct_api *clock_api;
26 const struct mod_css_clock_dev_config *config;
27 };
28
29 /* Module context */
30 struct css_clock_ctx {
31 struct css_clock_dev_ctx *dev_ctx_table;
32 unsigned int dev_count;
33 };
34
35 static struct css_clock_ctx module_ctx;
36
37 /*
38 * Static helper functions
39 */
40
compare_rate_entry(const void * a,const void * b)41 static int compare_rate_entry(const void *a, const void *b)
42 {
43 struct mod_css_clock_rate *key = (struct mod_css_clock_rate *)a;
44 struct mod_css_clock_rate *element = (struct mod_css_clock_rate *)b;
45
46 return (key->rate - element->rate);
47 }
48
get_rate_entry(struct css_clock_dev_ctx * ctx,uint64_t target_rate,struct mod_css_clock_rate ** entry)49 static int get_rate_entry(struct css_clock_dev_ctx *ctx, uint64_t target_rate,
50 struct mod_css_clock_rate **entry)
51 {
52 struct mod_css_clock_rate *current_rate_entry;
53
54 if (ctx == NULL)
55 return FWK_E_PARAM;
56 if (entry == NULL)
57 return FWK_E_PARAM;
58
59 /* Perform a binary search to find the entry matching the requested rate */
60 current_rate_entry = (struct mod_css_clock_rate *) bsearch(&target_rate,
61 ctx->config->rate_table, ctx->config->rate_count,
62 sizeof(struct mod_css_clock_rate), compare_rate_entry);
63
64 if (current_rate_entry == NULL)
65 return FWK_E_PARAM;
66
67 *entry = current_rate_entry;
68 return FWK_SUCCESS;
69 }
70
set_rate_indexed(struct css_clock_dev_ctx * ctx,uint64_t rate,enum mod_clock_round_mode round_mode)71 static int set_rate_indexed(struct css_clock_dev_ctx *ctx, uint64_t rate,
72 enum mod_clock_round_mode round_mode)
73 {
74 int status;
75 unsigned int i;
76 struct mod_css_clock_rate *rate_entry;
77
78 if (ctx == NULL)
79 return FWK_E_PARAM;
80
81 /* Look up the divider and source settings */
82 status = get_rate_entry(ctx, rate, &rate_entry);
83 if (status != FWK_SUCCESS)
84 goto exit;
85
86 /* Switch each member clock away from the PLL source */
87 for (i = 0; i < ctx->config->member_count; i++) {
88 status = ctx->clock_api->set_source(ctx->config->member_table[i],
89 ctx->config->clock_switching_source);
90 if (status != FWK_SUCCESS)
91 goto exit;
92
93 status = ctx->clock_api->set_div(ctx->config->member_table[i],
94 rate_entry->clock_div_type,
95 rate_entry->clock_div);
96 if (status != FWK_SUCCESS)
97 goto exit;
98
99 if (ctx->config->modulation_supported) {
100 status = ctx->clock_api->set_mod(ctx->config->member_table[i],
101 rate_entry->clock_mod_numerator,
102 rate_entry->clock_mod_denominator);
103 if (status != FWK_SUCCESS)
104 goto exit;
105 }
106 }
107
108 /* Change the PLL to the desired rate */
109 status = ctx->pll_api->set_rate(ctx->config->pll_id, rate_entry->pll_rate,
110 MOD_CLOCK_ROUND_MODE_NONE);
111 if (status != FWK_SUCCESS)
112 goto exit;
113
114 /* Return each member clock back to the PLL source */
115 for (i = 0; i < ctx->config->member_count; i++) {
116 status = ctx->clock_api->set_source(ctx->config->member_table[i],
117 rate_entry->clock_source);
118 if (status != FWK_SUCCESS)
119 goto exit;
120 }
121
122 exit:
123 if (status == FWK_SUCCESS)
124 ctx->current_rate = rate;
125 return status;
126 }
127
set_rate_non_indexed(struct css_clock_dev_ctx * ctx,uint64_t rate,enum mod_clock_round_mode round_mode)128 static int set_rate_non_indexed(struct css_clock_dev_ctx *ctx, uint64_t rate,
129 enum mod_clock_round_mode round_mode)
130 {
131 int status;
132 unsigned int i;
133
134 if (ctx == NULL)
135 return FWK_E_PARAM;
136
137 /* Switch each member clock away from the PLL source */
138 for (i = 0; i < ctx->config->member_count; i++) {
139 status = ctx->clock_api->set_source(ctx->config->member_table[i],
140 ctx->config->clock_switching_source);
141 if (status != FWK_SUCCESS)
142 goto exit;
143 }
144
145 /* Change the PLL to the desired rate */
146 status = ctx->pll_api->set_rate(ctx->config->pll_id, rate, round_mode);
147 if (status != FWK_SUCCESS)
148 goto exit;
149
150 /* Return each member clock back to the PLL source */
151 for (i = 0; i < ctx->config->member_count; i++) {
152 status = ctx->clock_api->set_source(ctx->config->member_table[i],
153 ctx->config->clock_default_source);
154 if (status != FWK_SUCCESS)
155 goto exit;
156 }
157
158 exit:
159 if (status == FWK_SUCCESS)
160 ctx->current_rate = rate;
161 return status;
162 }
163
164 /*
165 * Module API functions
166 */
167
css_clock_set_rate(fwk_id_t dev_id,uint64_t rate,enum mod_clock_round_mode round_mode)168 static int css_clock_set_rate(fwk_id_t dev_id, uint64_t rate,
169 enum mod_clock_round_mode round_mode)
170 {
171 struct css_clock_dev_ctx *ctx;
172
173 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
174
175 if (ctx->config->clock_type == MOD_CSS_CLOCK_TYPE_INDEXED)
176 return set_rate_indexed(ctx, rate, round_mode);
177 else
178 return set_rate_non_indexed(ctx, rate, round_mode);
179 }
180
css_clock_get_rate(fwk_id_t dev_id,uint64_t * rate)181 static int css_clock_get_rate(fwk_id_t dev_id, uint64_t *rate)
182 {
183 struct css_clock_dev_ctx *ctx;
184
185 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
186 *rate = ctx->current_rate;
187
188 return FWK_SUCCESS;
189 }
190
css_clock_get_rate_from_index(fwk_id_t dev_id,unsigned int rate_index,uint64_t * rate)191 static int css_clock_get_rate_from_index(fwk_id_t dev_id,
192 unsigned int rate_index,
193 uint64_t *rate)
194 {
195 struct css_clock_dev_ctx *ctx;
196
197 if (rate == NULL)
198 return FWK_E_PARAM;
199
200 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
201
202 if (rate_index >= ctx->config->rate_count)
203 return FWK_E_PARAM;
204
205 if (ctx->config->clock_type == MOD_CSS_CLOCK_TYPE_INDEXED) {
206 *rate = ctx->config->rate_table[rate_index].rate;
207 return FWK_SUCCESS;
208 } else
209 return FWK_E_SUPPORT;
210 }
211
css_clock_set_state(fwk_id_t dev_id,enum mod_clock_state state)212 static int css_clock_set_state(fwk_id_t dev_id, enum mod_clock_state state)
213 {
214 if (state == MOD_CLOCK_STATE_RUNNING)
215 return FWK_SUCCESS; /* CSS clocks are always running */
216
217 /* CSS clocks cannot be turned off */
218 return FWK_E_SUPPORT;
219 }
220
css_clock_get_state(fwk_id_t dev_id,enum mod_clock_state * state)221 static int css_clock_get_state(fwk_id_t dev_id, enum mod_clock_state *state)
222 {
223 *state = MOD_CLOCK_STATE_RUNNING;
224
225 return FWK_SUCCESS;
226 }
227
css_clock_get_range(fwk_id_t dev_id,struct mod_clock_range * range)228 static int css_clock_get_range(fwk_id_t dev_id, struct mod_clock_range *range)
229 {
230 struct css_clock_dev_ctx *ctx;
231
232 if (range == NULL)
233 return FWK_E_PARAM;
234
235 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
236
237 if (ctx->config->clock_type == MOD_CSS_CLOCK_TYPE_INDEXED) {
238 range->rate_type = MOD_CLOCK_RATE_TYPE_DISCRETE;
239 range->min = ctx->config->rate_table[0].rate;
240 range->max = ctx->config->rate_table[ctx->config->rate_count - 1].rate;
241 range->rate_count = ctx->config->rate_count;
242 return FWK_SUCCESS;
243 } else
244 return ctx->pll_api->get_range(ctx->config->pll_id, range);
245 }
246
css_clock_power_state_change(fwk_id_t dev_id,unsigned int next_state)247 static int css_clock_power_state_change(
248 fwk_id_t dev_id,
249 unsigned int next_state)
250 {
251 int status;
252 unsigned int clock_idx;
253 struct css_clock_dev_ctx *ctx;
254 const struct mod_css_clock_dev_config *dev_config;
255
256 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
257 dev_config = ctx->config;
258
259 /* The group's clock driver is not required to handle this transition */
260 if (ctx->clock_api->process_power_transition != NULL) {
261 for (clock_idx = 0; clock_idx < dev_config->member_count; clock_idx++) {
262 /* Allow the member clock's driver to perform any required
263 * processing */
264 status = ctx->clock_api->process_power_transition(
265 dev_config->member_table[clock_idx], next_state);
266
267 if (status != FWK_SUCCESS)
268 return status;
269 }
270 }
271
272 if (next_state == MOD_PD_STATE_ON) {
273 if (ctx->initialized) {
274 /* Restore all clocks in the group to the last frequency */
275 return css_clock_set_rate(dev_id, ctx->current_rate,
276 MOD_CLOCK_ROUND_MODE_NONE);
277 } else {
278 ctx->initialized = true;
279 /* Set all clocks in the group to the initial frequency */
280 return css_clock_set_rate(dev_id, dev_config->initial_rate,
281 MOD_CLOCK_ROUND_MODE_NONE);
282 }
283 }
284
285 return FWK_SUCCESS;
286 }
287
css_clock_pending_power_state_change(fwk_id_t dev_id,unsigned int current_state,unsigned int next_state)288 static int css_clock_pending_power_state_change(
289 fwk_id_t dev_id,
290 unsigned int current_state,
291 unsigned int next_state)
292 {
293 int status;
294 unsigned int clock_idx;
295 struct css_clock_dev_ctx *ctx;
296 const struct mod_css_clock_dev_config *dev_config;
297
298 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
299 dev_config = ctx->config;
300
301 /* The group's clock driver is not required to handle this transition */
302 if (ctx->clock_api->process_pending_power_transition != NULL) {
303 for (clock_idx = 0; clock_idx < dev_config->member_count; clock_idx++) {
304 /* Allow the member clock's driver to perform any required
305 * processing */
306 status = ctx->clock_api->process_pending_power_transition(
307 dev_config->member_table[clock_idx], current_state, next_state);
308
309 if (status != FWK_SUCCESS)
310 return status;
311 }
312 }
313
314 /* Nothing specific to be done in this driver */
315 return FWK_SUCCESS;
316 }
317
318 static const struct mod_clock_drv_api api_clock = {
319 .set_rate = css_clock_set_rate,
320 .get_rate = css_clock_get_rate,
321 .get_rate_from_index = css_clock_get_rate_from_index,
322 .set_state = css_clock_set_state,
323 .get_state = css_clock_get_state,
324 .get_range = css_clock_get_range,
325 .process_power_transition = css_clock_power_state_change,
326 .process_pending_power_transition = css_clock_pending_power_state_change,
327 };
328
329 /*
330 * Framework handler functions
331 */
332
css_clock_init(fwk_id_t module_id,unsigned int element_count,const void * data)333 static int css_clock_init(fwk_id_t module_id, unsigned int element_count,
334 const void *data)
335 {
336 module_ctx.dev_count = element_count;
337
338 if (element_count == 0)
339 return FWK_SUCCESS;
340
341 module_ctx.dev_ctx_table = fwk_mm_calloc(element_count,
342 sizeof(struct css_clock_dev_ctx));
343 return FWK_SUCCESS;
344 }
345
css_clock_element_init(fwk_id_t element_id,unsigned int sub_element_count,const void * data)346 static int css_clock_element_init(fwk_id_t element_id,
347 unsigned int sub_element_count,
348 const void *data)
349 {
350 unsigned int i = 0;
351 uint64_t current_rate;
352 uint64_t last_rate = 0;
353 struct css_clock_dev_ctx *ctx;
354 const struct mod_css_clock_dev_config *dev_config = data;
355
356 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(element_id);
357
358 if (dev_config->clock_type == MOD_CSS_CLOCK_TYPE_INDEXED) {
359 /* Verify that the rate entries in the lookup table are ordered */
360 while (i < dev_config->rate_count) {
361 current_rate = dev_config->rate_table[i].rate;
362
363 /* The rate entries must be in ascending order */
364 if (current_rate < last_rate)
365 return FWK_E_DATA;
366
367 last_rate = current_rate;
368 i++;
369 }
370 }
371
372 ctx->config = dev_config;
373 ctx->current_rate = ctx->config->initial_rate;
374
375 return FWK_SUCCESS;
376 }
377
css_clock_bind(fwk_id_t id,unsigned int round)378 static int css_clock_bind(fwk_id_t id, unsigned int round)
379 {
380 int status;
381 struct css_clock_dev_ctx *ctx;
382 const struct mod_css_clock_dev_config *config;
383
384 if (round == 1)
385 return FWK_SUCCESS;
386
387 if (fwk_module_is_valid_module_id(id))
388 /* No module-level binding required */
389 return FWK_SUCCESS;
390
391 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(id);
392 config = ctx->config;
393
394 /* Ensure that the group has at least one member */
395 if (config->member_count == 0)
396 return FWK_E_DATA;
397
398 /* Bind to the group's common PLL driver */
399 status = fwk_module_bind(config->pll_id, config->pll_api_id,
400 &ctx->pll_api);
401 if (status != FWK_SUCCESS)
402 return status;
403
404 /* Bind to the API used to control the clocks in the group */
405 status = fwk_module_bind(config->member_table[0],
406 config->member_api_id, &ctx->clock_api);
407 if (status != FWK_SUCCESS)
408 return status;
409
410 return FWK_SUCCESS;
411 }
412
css_clock_process_bind_request(fwk_id_t source_id,fwk_id_t target_id,fwk_id_t api_id,const void ** api)413 static int css_clock_process_bind_request(fwk_id_t source_id,
414 fwk_id_t target_id, fwk_id_t api_id,
415 const void **api)
416 {
417 if (fwk_id_get_api_idx(api_id) != MOD_CSS_CLOCK_API_TYPE_CLOCK)
418 /* The requested API is not supported. */
419 return FWK_E_ACCESS;
420
421 *api = &api_clock;
422 return FWK_SUCCESS;
423 }
424
425 const struct fwk_module module_css_clock = {
426 .type = FWK_MODULE_TYPE_DRIVER,
427 .api_count = MOD_CSS_CLOCK_API_COUNT,
428 .event_count = 0,
429 .init = css_clock_init,
430 .element_init = css_clock_element_init,
431 .bind = css_clock_bind,
432 .process_bind_request = css_clock_process_bind_request,
433 };
434