1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "ld_dvfs_handler.h"
8 #include "ld_dvfs.h"
9
10 #include <hal/nrf_hsfll.h>
11 #include <nrfs_dvfs.h>
12 #include <nrfs_backend_ipc_service.h>
13
14 #include <zephyr/kernel.h>
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_DECLARE(LD_DVFS_LIB, CONFIG_LOCAL_DOMAIN_DVFS_LIB_LOG_LEVEL);
17
18 static K_SEM_DEFINE(dvfs_service_sync_sem, 0, 1);
19 static K_SEM_DEFINE(dvfs_service_idle_sem, 0, 1);
20
21 #define DVFS_SERV_HDL_INIT_DONE_BIT_POS (0)
22 #define DVFS_SERV_HDL_FREQ_CHANGE_REQ_PENDING_BIT_POS (1)
23
24 static atomic_t dvfs_service_handler_state_bits;
25 static volatile enum dvfs_frequency_setting current_freq_setting;
26 static volatile enum dvfs_frequency_setting requested_freq_setting;
27 static dvfs_service_handler_callback dvfs_frequency_change_applied_clb;
28
dvfs_service_handler_set_state_bit(uint32_t bit_pos)29 static void dvfs_service_handler_set_state_bit(uint32_t bit_pos)
30 {
31 atomic_set_bit(&dvfs_service_handler_state_bits, bit_pos);
32 }
33
dvfs_service_handler_clear_state_bit(uint32_t bit_pos)34 static void dvfs_service_handler_clear_state_bit(uint32_t bit_pos)
35 {
36 atomic_clear_bit(&dvfs_service_handler_state_bits, bit_pos);
37 }
38
dvfs_service_handler_get_state_bit(uint32_t bit_pos)39 static bool dvfs_service_handler_get_state_bit(uint32_t bit_pos)
40 {
41 return atomic_test_bit(&dvfs_service_handler_state_bits, bit_pos);
42 }
43
dvfs_service_handler_init_done(void)44 static bool dvfs_service_handler_init_done(void)
45 {
46 return dvfs_service_handler_get_state_bit(DVFS_SERV_HDL_INIT_DONE_BIT_POS);
47 }
48
dvfs_service_handler_freq_change_req_pending(void)49 static bool dvfs_service_handler_freq_change_req_pending(void)
50 {
51 return dvfs_service_handler_get_state_bit(DVFS_SERV_HDL_FREQ_CHANGE_REQ_PENDING_BIT_POS);
52 }
53
dvfs_service_handler_nrfs_error_check(nrfs_err_t err)54 static void dvfs_service_handler_nrfs_error_check(nrfs_err_t err)
55 {
56 if (err != NRFS_SUCCESS) {
57 LOG_ERR("Failed with nrfs error: %d", err);
58 }
59 }
60
dvfs_service_handler_error(int err)61 static void dvfs_service_handler_error(int err)
62 {
63 if (err != 0) {
64 LOG_ERR("Failed with error: %d", err);
65 }
66 }
67
get_next_context(void)68 static uint32_t *get_next_context(void)
69 {
70 static uint32_t ctx;
71
72 ctx++;
73 return &ctx;
74 }
75
dvfs_service_handler_freq_setting_allowed(enum dvfs_frequency_setting freq_setting)76 static bool dvfs_service_handler_freq_setting_allowed(enum dvfs_frequency_setting freq_setting)
77 {
78 if (freq_setting == DVFS_FREQ_HIGH || freq_setting == DVFS_FREQ_MEDLOW ||
79 freq_setting == DVFS_FREQ_LOW) {
80 return true;
81 }
82
83 return false;
84 }
85
dvfs_service_handler_get_current_oppoint(void)86 static enum dvfs_frequency_setting dvfs_service_handler_get_current_oppoint(void)
87 {
88 LOG_DBG("Current LD freq setting: %d", current_freq_setting);
89 return current_freq_setting;
90 }
91
dvfs_service_handler_get_requested_oppoint(void)92 static enum dvfs_frequency_setting dvfs_service_handler_get_requested_oppoint(void)
93 {
94 LOG_DBG("Requested LD freq setting: %d", requested_freq_setting);
95 return requested_freq_setting;
96 }
97
98 /* Function to check if current operation is down-scaling */
dvfs_service_handler_is_downscaling(enum dvfs_frequency_setting target_freq_setting)99 static bool dvfs_service_handler_is_downscaling(enum dvfs_frequency_setting target_freq_setting)
100 {
101 if (dvfs_service_handler_freq_setting_allowed(target_freq_setting)) {
102 LOG_DBG("Checking if downscaling %s",
103 (dvfs_service_handler_get_current_oppoint() < target_freq_setting) ? "YES" :
104 "NO");
105 return dvfs_service_handler_get_current_oppoint() < target_freq_setting;
106 }
107
108 return false;
109 }
110
111 /* Function handling steps for scaling preparation. */
dvfs_service_handler_prepare_to_scale(enum dvfs_frequency_setting oppoint_freq)112 static void dvfs_service_handler_prepare_to_scale(enum dvfs_frequency_setting oppoint_freq)
113 {
114 LOG_DBG("Prepare to scale, oppoint freq %d", oppoint_freq);
115 enum dvfs_frequency_setting new_oppoint = oppoint_freq;
116 enum dvfs_frequency_setting current_oppoint = dvfs_service_handler_get_current_oppoint();
117
118 if (new_oppoint == current_oppoint) {
119 LOG_DBG("New oppoint is same as previous, no change");
120 } else {
121 ld_dvfs_configure_abb_for_transition(current_oppoint, new_oppoint);
122
123 if (dvfs_service_handler_is_downscaling(new_oppoint)) {
124 int32_t err = ld_dvfs_configure_hsfll(new_oppoint);
125
126 if (err != 0) {
127 dvfs_service_handler_error(err);
128 }
129 }
130 }
131 }
132
133 /* Do background job during scaling process (e.g. increased power consumption during down-scale). */
dvfs_service_handler_scaling_background_job(enum dvfs_frequency_setting oppoint_freq)134 static void dvfs_service_handler_scaling_background_job(enum dvfs_frequency_setting oppoint_freq)
135 {
136 LOG_DBG("Perform scaling background job if needed.");
137 if (dvfs_service_handler_is_downscaling(oppoint_freq)) {
138 k_sem_give(&dvfs_service_idle_sem);
139 }
140 }
141
142 /* Update MDK variable which is used by nrfx_coredep_delay_us (k_busy_wait). */
dvfs_service_update_core_clock(enum dvfs_frequency_setting oppoint_freq)143 static void dvfs_service_update_core_clock(enum dvfs_frequency_setting oppoint_freq)
144 {
145 extern uint32_t SystemCoreClock;
146
147 SystemCoreClock = oppoint_freq == DVFS_FREQ_HIGH ? 320000000 :
148 oppoint_freq == DVFS_FREQ_MEDLOW ? 128000000 : 64000000;
149 }
150
151 /* Perform scaling finnish procedure. */
dvfs_service_handler_scaling_finish(enum dvfs_frequency_setting oppoint_freq)152 static void dvfs_service_handler_scaling_finish(enum dvfs_frequency_setting oppoint_freq)
153 {
154
155 LOG_DBG("Scaling finnish oppoint freq %d", oppoint_freq);
156 ld_dvfs_scaling_finish(dvfs_service_handler_is_downscaling(oppoint_freq));
157 if (!dvfs_service_handler_is_downscaling(oppoint_freq)) {
158 int32_t err = ld_dvfs_configure_hsfll(oppoint_freq);
159
160 if (err != 0) {
161 dvfs_service_handler_error(err);
162 }
163 }
164 dvfs_service_handler_clear_state_bit(DVFS_SERV_HDL_FREQ_CHANGE_REQ_PENDING_BIT_POS);
165 current_freq_setting = oppoint_freq;
166 dvfs_service_update_core_clock(oppoint_freq);
167 LOG_DBG("Current LD freq setting: %d", current_freq_setting);
168 if (dvfs_frequency_change_applied_clb) {
169 dvfs_frequency_change_applied_clb(current_freq_setting);
170 }
171 }
172
173 /* Function to set hsfll to highest frequency when switched to ABB. */
dvfs_service_handler_set_initial_hsfll_config(void)174 static void dvfs_service_handler_set_initial_hsfll_config(void)
175 {
176 int32_t err = ld_dvfs_configure_hsfll(DVFS_FREQ_HIGH);
177
178 current_freq_setting = DVFS_FREQ_HIGH;
179 requested_freq_setting = DVFS_FREQ_HIGH;
180 if (err != 0) {
181 dvfs_service_handler_error(err);
182 }
183 }
184
185 /* Timer to add additional delay to finish downscale procedure when domain other than secure */
186 #if !defined(NRF_SECURE)
187 #define SCALING_FINISH_DELAY_TIMEOUT_US \
188 K_USEC(CONFIG_NRFS_LOCAL_DOMAIN_DOWNSCALE_FINISH_DELAY_TIMEOUT_US)
189
dvfs_service_handler_scaling_finish_delay_timeout(struct k_timer * timer)190 static void dvfs_service_handler_scaling_finish_delay_timeout(struct k_timer *timer)
191 {
192
193 dvfs_service_handler_scaling_finish(
194 *(enum dvfs_frequency_setting *)k_timer_user_data_get(timer));
195 }
196
197 K_TIMER_DEFINE(dvfs_service_scaling_finish_delay_timer,
198 dvfs_service_handler_scaling_finish_delay_timeout, NULL);
199 #endif
200
201 /* DVFS event handler callback function.*/
nrfs_dvfs_evt_handler(nrfs_dvfs_evt_t const * p_evt,void * context)202 static void nrfs_dvfs_evt_handler(nrfs_dvfs_evt_t const *p_evt, void *context)
203 {
204 LOG_DBG("%s", __func__);
205 switch (p_evt->type) {
206 case NRFS_DVFS_EVT_INIT_PREPARATION:
207 LOG_DBG("DVFS handler EVT_INIT_PREPARATION");
208 #if defined(NRF_SECURE)
209 ld_dvfs_clear_zbb();
210 dvfs_service_handler_nrfs_error_check(
211 nrfs_dvfs_init_complete_request(get_next_context()));
212 LOG_DBG("DVFS handler EVT_INIT_PREPARATION handled");
213 #else
214 LOG_ERR("DVFS handler - unexpected EVT_INIT_PREPARATION");
215 #endif
216 break;
217 case NRFS_DVFS_EVT_INIT_DONE:
218 LOG_DBG("DVFS handler EVT_INIT_DONE");
219 dvfs_service_handler_set_initial_hsfll_config();
220 dvfs_service_handler_set_state_bit(DVFS_SERV_HDL_INIT_DONE_BIT_POS);
221 k_sem_give(&dvfs_service_sync_sem);
222 LOG_DBG("DVFS handler EVT_INIT_DONE handled");
223 break;
224 case NRFS_DVFS_EVT_OPPOINT_REQ_CONFIRMED:
225 /* Optional confirmation from sysctrl, wait for oppoint.*/
226 dvfs_service_handler_clear_state_bit(DVFS_SERV_HDL_FREQ_CHANGE_REQ_PENDING_BIT_POS);
227 LOG_DBG("DVFS handler EVT_OPPOINT_REQ_CONFIRMED %d", (uint32_t)p_evt->freq);
228 if (dvfs_service_handler_get_requested_oppoint() == p_evt->freq) {
229 dvfs_service_update_core_clock(p_evt->freq);
230 if (dvfs_frequency_change_applied_clb) {
231 dvfs_frequency_change_applied_clb(p_evt->freq);
232 }
233 }
234 break;
235 case NRFS_DVFS_EVT_OPPOINT_SCALING_PREPARE:
236 /*Target oppoint will be received here.*/
237 LOG_DBG("DVFS handler EVT_OPPOINT_SCALING_PREPARE");
238 #if !defined(NRF_SECURE)
239 if (dvfs_service_handler_is_downscaling(p_evt->freq)) {
240 #endif
241 dvfs_service_handler_prepare_to_scale(p_evt->freq);
242 dvfs_service_handler_nrfs_error_check(
243 nrfs_dvfs_ready_to_scale(get_next_context()));
244 dvfs_service_handler_scaling_background_job(p_evt->freq);
245 LOG_DBG("DVFS handler EVT_OPPOINT_SCALING_PREPARE handled");
246 #if !defined(NRF_SECURE)
247 /* Additional delay for downscale to finish on secdom side */
248 static enum dvfs_frequency_setting freq;
249
250 freq = p_evt->freq;
251 k_timer_user_data_set(&dvfs_service_scaling_finish_delay_timer,
252 (void *)&freq);
253 k_timer_start(&dvfs_service_scaling_finish_delay_timer,
254 SCALING_FINISH_DELAY_TIMEOUT_US, K_NO_WAIT);
255 } else {
256 LOG_ERR("DVFS handler - unexpected EVT_OPPOINT_SCALING_PREPARE");
257 }
258 #endif
259 break;
260 case NRFS_DVFS_EVT_OPPOINT_SCALING_DONE:
261 LOG_DBG("DVFS handler EVT_OPPOINT_SCALING_DONE");
262 dvfs_service_handler_scaling_finish(p_evt->freq);
263 LOG_DBG("DVFS handler EVT_OPPOINT_SCALING_DONE handled");
264 break;
265 case NRFS_DVFS_EVT_REJECT:
266 LOG_ERR("DVFS handler - request rejected");
267 break;
268 default:
269 LOG_ERR("DVFS handler - unexpected event: 0x%x", p_evt->type);
270 break;
271 }
272 }
273
274 /* Task to handle dvfs init procedure. */
dvfs_service_handler_task(void * dummy0,void * dummy1,void * dummy2)275 static void dvfs_service_handler_task(void *dummy0, void *dummy1, void *dummy2)
276 {
277 ARG_UNUSED(dummy0);
278 ARG_UNUSED(dummy1);
279 ARG_UNUSED(dummy2);
280
281 LOG_DBG("Trim ABB for default voltage.");
282 ld_dvfs_init();
283
284 LOG_DBG("Waiting for backend init");
285 /* Wait for ipc initialization */
286 nrfs_backend_wait_for_connection(K_FOREVER);
287
288 nrfs_err_t status;
289
290 LOG_DBG("nrfs_dvfs_init");
291 status = nrfs_dvfs_init(nrfs_dvfs_evt_handler);
292 dvfs_service_handler_nrfs_error_check(status);
293
294 LOG_DBG("nrfs_dvfs_init_prepare_request");
295 status = nrfs_dvfs_init_prepare_request(get_next_context());
296 dvfs_service_handler_nrfs_error_check(status);
297
298 /* Wait for init*/
299 k_sem_take(&dvfs_service_sync_sem, K_FOREVER);
300
301 LOG_DBG("DVFS init done.");
302
303 #if defined(CONFIG_NRFS_LOCAL_DOMAIN_DVFS_SCALE_DOWN_AFTER_INIT)
304 LOG_DBG("Requesting lowest frequency oppoint.");
305 dvfs_service_handler_change_freq_setting(DVFS_FREQ_LOW);
306 #endif
307
308 while (1) {
309 k_sem_take(&dvfs_service_idle_sem, K_FOREVER);
310 /* perform background processing */
311 ld_dvfs_scaling_background_process(true);
312 }
313 }
314
315 K_THREAD_DEFINE(dvfs_service_handler_task_id,
316 CONFIG_NRFS_LOCAL_DOMAIN_DVFS_HANDLER_TASK_STACK_SIZE,
317 dvfs_service_handler_task,
318 NULL,
319 NULL,
320 NULL,
321 CONFIG_NRFS_LOCAL_DOMAIN_DVFS_HANDLER_TASK_PRIORITY,
322 0,
323 0);
324
dvfs_service_handler_change_freq_setting(enum dvfs_frequency_setting freq_setting)325 int32_t dvfs_service_handler_change_freq_setting(enum dvfs_frequency_setting freq_setting)
326 {
327 if (!dvfs_service_handler_init_done()) {
328 LOG_WRN("Init not done!");
329 return -EAGAIN;
330 }
331
332 if (!dvfs_service_handler_freq_setting_allowed(freq_setting)) {
333 LOG_ERR("Requested frequency setting %d not supported.", freq_setting);
334 return -ENXIO;
335 }
336
337 if (dvfs_service_handler_freq_change_req_pending()) {
338 LOG_DBG("Frequency change request pending.");
339 return -EBUSY;
340 }
341
342 dvfs_service_handler_set_state_bit(DVFS_SERV_HDL_FREQ_CHANGE_REQ_PENDING_BIT_POS);
343 requested_freq_setting = freq_setting;
344
345 nrfs_err_t status = nrfs_dvfs_oppoint_request(freq_setting, get_next_context());
346
347 if (status != NRFS_SUCCESS) {
348 dvfs_service_handler_clear_state_bit(DVFS_SERV_HDL_FREQ_CHANGE_REQ_PENDING_BIT_POS);
349 }
350
351 dvfs_service_handler_nrfs_error_check(status);
352
353 return status;
354 }
355
dvfs_service_handler_register_freq_setting_applied_callback(dvfs_service_handler_callback clb)356 void dvfs_service_handler_register_freq_setting_applied_callback(dvfs_service_handler_callback clb)
357 {
358 if (clb) {
359 LOG_DBG("Registered frequency applied callback");
360 dvfs_frequency_change_applied_clb = clb;
361 } else {
362 LOG_ERR("Invalid callback function provided!");
363 }
364 }
365
