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_power_domain.h>
10 #include <mod_system_pll.h>
11
12 #include <fwk_id.h>
13 #include <fwk_macros.h>
14 #include <fwk_mm.h>
15 #include <fwk_module.h>
16 #include <fwk_status.h>
17
18 #include <stddef.h>
19 #include <stdint.h>
20
21 /* Device context */
22 struct system_pll_dev_ctx {
23 bool initialized;
24 uint64_t current_rate;
25 enum mod_clock_state current_state;
26 const struct mod_system_pll_dev_config *config;
27 };
28
29 /* Module context */
30 struct system_pll_ctx {
31 struct system_pll_dev_ctx *dev_ctx_table;
32 unsigned int dev_count;
33 };
34
35 static struct system_pll_ctx module_ctx;
36
37 /*
38 * Static helper functions
39 */
40
41 /*
42 * Given a frequency (Hz), return the period (picoseconds) of a half cycle.
43 *
44 * Note: For performance reasons, 32-bit math is used for the conversion. This
45 * may cause a loss of precision if the given frequency is not a multiple
46 * of 1 KHz.
47 */
freq_to_half_cycle_ps(unsigned int freq_hz)48 static unsigned int freq_to_half_cycle_ps(unsigned int freq_hz)
49 {
50 unsigned int freq_khz;
51
52 /* Check if the given frequency is a multiple of 1 KHz */
53 if (freq_hz % MOD_SYSTEM_PLL_MIN_INTERVAL != 0)
54 return 0;
55
56 freq_khz = freq_hz / FWK_KHZ;
57 if (freq_khz == 0)
58 return 0;
59
60 /*
61 * Legend:
62 * s = seconds
63 * P = Half cycle period in picoseconds
64 * Fh = Frequency in hertz
65 * Fk = Frequency in kilohertz
66 *
67 * Starting from "Period = Time / Frequency"
68 * General equation for half cycle in picoseconds:
69 * P = ((1s / Fh ) / 2) * 10^12
70 * To avoid decimal calculations, re-arrange and simplify equation:
71 * P = 10^12 / 2 * Fh
72 * To avoid dividend overflowing a 32-bit storage, dividend and divisor can
73 * be divided by 10^3:
74 * P = (10^12 / 10^3) / (2 * Fh / 10^3)
75 * Given Fk = Fh / 10^3, the equation can be further simplified as:
76 * P = 10^9 / 2 * Fk
77 * P = 5*10^8 / Fk
78 */
79 return 500000000UL / freq_khz;
80 }
81
82 /*
83 * Clock driver API functions
84 */
85
system_pll_set_rate(fwk_id_t dev_id,uint64_t rate,enum mod_clock_round_mode round_mode)86 static int system_pll_set_rate(fwk_id_t dev_id, uint64_t rate,
87 enum mod_clock_round_mode round_mode)
88 {
89 uint64_t rounded_rate;
90 uint64_t rounded_rate_alt;
91 unsigned int picoseconds;
92 struct system_pll_dev_ctx *ctx;
93
94 if (!fwk_module_is_valid_element_id(dev_id))
95 return FWK_E_PARAM;
96
97 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
98
99 if (ctx->current_state == MOD_CLOCK_STATE_STOPPED)
100 return FWK_E_PWRSTATE;
101
102 /* If the given rate is not attainable as-is then round as requested */
103 if ((rate % ctx->config->min_step) > 0) {
104 switch (round_mode) {
105 case MOD_CLOCK_ROUND_MODE_NONE:
106 return FWK_E_RANGE;
107 case MOD_CLOCK_ROUND_MODE_NEAREST:
108 rounded_rate = FWK_ALIGN_PREVIOUS(rate, ctx->config->min_step);
109 rounded_rate_alt = FWK_ALIGN_NEXT(rate, ctx->config->min_step);
110
111 /* Select the rounded rate that is closest to the given rate */
112 if ((rate - rounded_rate) > (rounded_rate_alt - rate))
113 rounded_rate = rounded_rate_alt;
114 break;
115 case MOD_CLOCK_ROUND_MODE_DOWN:
116 rounded_rate = FWK_ALIGN_PREVIOUS(rate, ctx->config->min_step);
117 break;
118 case MOD_CLOCK_ROUND_MODE_UP:
119 rounded_rate = FWK_ALIGN_NEXT(rate, ctx->config->min_step);
120 break;
121 default:
122 return FWK_E_SUPPORT;
123 }
124 } else
125 rounded_rate = rate;
126
127 if (rounded_rate < ctx->config->min_rate)
128 return FWK_E_RANGE;
129 if (rounded_rate > ctx->config->max_rate)
130 return FWK_E_RANGE;
131
132 picoseconds = freq_to_half_cycle_ps(rounded_rate);
133
134 if (picoseconds == 0)
135 return FWK_E_RANGE;
136
137 *ctx->config->control_reg = picoseconds;
138
139 if (ctx->config->status_reg != NULL) {
140 /* Wait until the PLL has locked */
141 while ((*ctx->config->status_reg & ctx->config->lock_flag_mask) == 0)
142 continue;
143 }
144
145 ctx->current_rate = rounded_rate;
146
147 return FWK_SUCCESS;
148 }
149
system_pll_get_rate(fwk_id_t dev_id,uint64_t * rate)150 static int system_pll_get_rate(fwk_id_t dev_id, uint64_t *rate)
151 {
152 struct system_pll_dev_ctx *ctx;
153
154 if (!fwk_module_is_valid_element_id(dev_id))
155 return FWK_E_PARAM;
156 if (rate == NULL)
157 return FWK_E_PARAM;
158
159 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
160 *rate = ctx->current_rate;
161
162 return FWK_SUCCESS;
163 }
164
system_pll_get_rate_from_index(fwk_id_t dev_id,unsigned int rate_index,uint64_t * rate)165 static int system_pll_get_rate_from_index(fwk_id_t dev_id,
166 unsigned int rate_index,
167 uint64_t *rate)
168 {
169 /* PLLs have a continuous range of rates and are not indexed */
170 return FWK_E_SUPPORT;
171 }
172
system_pll_set_state(fwk_id_t dev_id,enum mod_clock_state state)173 static int system_pll_set_state(fwk_id_t dev_id, enum mod_clock_state state)
174 {
175 if (state == MOD_CLOCK_STATE_RUNNING)
176 return FWK_SUCCESS;
177
178 /* PLLs can only be stopped by a parent power domain state change. */
179 return FWK_E_SUPPORT;
180 }
181
system_pll_get_state(fwk_id_t dev_id,enum mod_clock_state * state)182 static int system_pll_get_state(fwk_id_t dev_id, enum mod_clock_state *state)
183 {
184 struct system_pll_dev_ctx *ctx;
185
186 if (!fwk_module_is_valid_element_id(dev_id))
187 return FWK_E_PARAM;
188 if (state == NULL)
189 return FWK_E_PARAM;
190
191 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
192 *state = ctx->current_state;
193
194 return FWK_SUCCESS;
195 }
196
system_pll_get_range(fwk_id_t dev_id,struct mod_clock_range * range)197 static int system_pll_get_range(fwk_id_t dev_id, struct mod_clock_range *range)
198 {
199 struct system_pll_dev_ctx *ctx;
200
201 if (!fwk_module_is_valid_element_id(dev_id))
202 return FWK_E_PARAM;
203 if (range == NULL)
204 return FWK_E_PARAM;
205
206 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
207
208 range->rate_type = MOD_CLOCK_RATE_TYPE_CONTINUOUS;
209 range->min = ctx->config->min_rate;
210 range->max = ctx->config->max_rate;
211 range->step = ctx->config->min_step;
212
213 return FWK_SUCCESS;
214 }
215
system_pll_power_state_change(fwk_id_t dev_id,unsigned int state)216 static int system_pll_power_state_change(
217 fwk_id_t dev_id,
218 unsigned int state)
219 {
220 uint64_t rate;
221 struct system_pll_dev_ctx *ctx;
222
223 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
224
225 if (state != MOD_PD_STATE_ON)
226 return FWK_SUCCESS;
227
228 ctx->current_state = MOD_CLOCK_STATE_RUNNING;
229
230 if (ctx->initialized) {
231 /* Restore the previous rate */
232 rate = ctx->current_rate;
233 } else {
234 /* Initialize the PLL to its default rate */
235 ctx->initialized = true;
236 rate = ctx->config->initial_rate;
237 }
238
239 return system_pll_set_rate(dev_id, rate, MOD_CLOCK_ROUND_MODE_NONE);
240 }
241
system_pll_power_state_pending_change(fwk_id_t dev_id,unsigned int current_state,unsigned int next_state)242 static int system_pll_power_state_pending_change(
243 fwk_id_t dev_id,
244 unsigned int current_state,
245 unsigned int next_state)
246 {
247 struct system_pll_dev_ctx *ctx;
248
249 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(dev_id);
250
251 if (next_state == MOD_PD_STATE_OFF) {
252 /* Just mark the PLL as stopped */
253 ctx->current_state = MOD_CLOCK_STATE_STOPPED;
254 }
255
256 return FWK_SUCCESS;
257 }
258
259 static const struct mod_clock_drv_api api_system_pll = {
260 .set_rate = system_pll_set_rate,
261 .get_rate = system_pll_get_rate,
262 .get_rate_from_index = system_pll_get_rate_from_index,
263 .set_state = system_pll_set_state,
264 .get_state = system_pll_get_state,
265 .get_range = system_pll_get_range,
266 .process_power_transition = system_pll_power_state_change,
267 .process_pending_power_transition = system_pll_power_state_pending_change,
268 };
269
270 /*
271 * Framework handler functions
272 */
273
system_pll_init(fwk_id_t module_id,unsigned int element_count,const void * data)274 static int system_pll_init(fwk_id_t module_id, unsigned int element_count,
275 const void *data)
276 {
277 module_ctx.dev_count = element_count;
278
279 if (element_count == 0)
280 return FWK_SUCCESS;
281
282 module_ctx.dev_ctx_table = fwk_mm_calloc(element_count,
283 sizeof(struct system_pll_dev_ctx));
284 return FWK_SUCCESS;
285 }
286
system_pll_element_init(fwk_id_t element_id,unsigned int unused,const void * data)287 static int system_pll_element_init(fwk_id_t element_id, unsigned int unused,
288 const void *data)
289 {
290 struct system_pll_dev_ctx *ctx;
291 const struct mod_system_pll_dev_config *dev_config = data;
292
293 ctx = module_ctx.dev_ctx_table + fwk_id_get_element_idx(element_id);
294
295 ctx->config = dev_config;
296
297 if (ctx->config->defer_initialization)
298 return FWK_SUCCESS;
299
300 ctx->initialized = true;
301 ctx->current_state = MOD_CLOCK_STATE_RUNNING;
302 return system_pll_set_rate(element_id, ctx->config->initial_rate,
303 MOD_CLOCK_ROUND_MODE_NONE);
304 }
305
system_pll_process_bind_request(fwk_id_t requester_id,fwk_id_t id,fwk_id_t api_type,const void ** api)306 static int system_pll_process_bind_request(fwk_id_t requester_id, fwk_id_t id,
307 fwk_id_t api_type, const void **api)
308 {
309 *api = &api_system_pll;
310 return FWK_SUCCESS;
311 }
312
313 const struct fwk_module module_system_pll = {
314 .type = FWK_MODULE_TYPE_DRIVER,
315 .api_count = MOD_SYSTEM_PLL_API_COUNT,
316 .event_count = 0,
317 .init = system_pll_init,
318 .element_init = system_pll_element_init,
319 .process_bind_request = system_pll_process_bind_request,
320 };
321