1 /* 2 * Copyright (c) 2013-2025, Arm Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7 #include <assert.h> 8 #include <stddef.h> 9 10 #include <arch.h> 11 #include <arch_helpers.h> 12 #include <common/bl_common.h> 13 #include <context.h> 14 #include <lib/cpus/errata.h> 15 #include <lib/el3_runtime/context_mgmt.h> 16 #include <lib/per_cpu/per_cpu.h> 17 #include <plat/common/platform.h> 18 19 #include "psci_private.h" 20 21 /* 22 * Check that PLATFORM_CORE_COUNT fits into the number of cores 23 * that can be represented by PSCI_MAX_CPUS_INDEX. 24 */ 25 CASSERT(PLATFORM_CORE_COUNT <= (PSCI_MAX_CPUS_INDEX + 1U), assert_psci_cores_overflow); 26 27 /******************************************************************************* 28 * Per cpu non-secure contexts used to program the architectural state prior 29 * return to the normal world. 30 * TODO: Use the memory allocator to set aside memory for the contexts instead 31 * of relying on platform defined constants. 32 ******************************************************************************/ 33 static PER_CPU_DEFINE(cpu_context_t, psci_ns_context); 34 static entry_point_info_t warmboot_ep_info[PLATFORM_CORE_COUNT]; 35 36 /****************************************************************************** 37 * Define the psci capability variable. 38 *****************************************************************************/ 39 unsigned int psci_caps; 40 41 /******************************************************************************* 42 * Function which initializes the 'psci_non_cpu_pd_nodes' or the 43 * 'psci_cpu_pd_nodes' corresponding to the power level. 44 ******************************************************************************/ 45 static void __init psci_init_pwr_domain_node(uint16_t node_idx, 46 unsigned int parent_idx, 47 unsigned char level) 48 { 49 if (level > PSCI_CPU_PWR_LVL) { 50 assert(node_idx < PSCI_NUM_NON_CPU_PWR_DOMAINS); 51 52 psci_non_cpu_pd_nodes[node_idx].level = level; 53 psci_lock_init(psci_non_cpu_pd_nodes, node_idx); 54 psci_non_cpu_pd_nodes[node_idx].parent_node = parent_idx; 55 psci_non_cpu_pd_nodes[node_idx].local_state = 56 PLAT_MAX_OFF_STATE; 57 } else { 58 psci_cpu_data_t *svc_cpu_data; 59 60 assert(node_idx < PLATFORM_CORE_COUNT); 61 62 PER_CPU_BY_INDEX(psci_cpu_pd_nodes, node_idx)->parent_node = parent_idx; 63 64 /* Initialize with an invalid mpidr */ 65 PER_CPU_BY_INDEX(psci_cpu_pd_nodes, node_idx)->mpidr = PSCI_INVALID_MPIDR; 66 67 svc_cpu_data = &get_cpu_data_by_index(node_idx, psci_svc_cpu_data); 68 69 /* Set the Affinity Info for the cores as OFF */ 70 svc_cpu_data->aff_info_state = AFF_STATE_OFF; 71 72 /* Default to the highest power level when the cpu is not suspending */ 73 svc_cpu_data->target_pwrlvl = PLAT_MAX_PWR_LVL; 74 75 /* Set the power state to OFF state */ 76 svc_cpu_data->local_state = PLAT_MAX_OFF_STATE; 77 78 psci_flush_dcache_range((uintptr_t)svc_cpu_data, 79 sizeof(*svc_cpu_data)); 80 81 cm_set_context_by_index(node_idx, 82 (void *) PER_CPU_BY_INDEX(psci_ns_context, 83 node_idx), 84 NON_SECURE); 85 } 86 } 87 88 /******************************************************************************* 89 * This functions updates cpu_start_idx and ncpus field for each of the node in 90 * psci_non_cpu_pd_nodes[]. It does so by comparing the parent nodes of each of 91 * the CPUs and check whether they match with the parent of the previous 92 * CPU. The basic assumption for this work is that children of the same parent 93 * are allocated adjacent indices. The platform should ensure this though proper 94 * mapping of the CPUs to indices via plat_core_pos_by_mpidr() and 95 * plat_my_core_pos() APIs. 96 *******************************************************************************/ 97 static void __init psci_update_pwrlvl_limits(void) 98 { 99 unsigned int cpu_idx; 100 int j; 101 unsigned int nodes_idx[PLAT_MAX_PWR_LVL] = {0}; 102 unsigned int temp_index[PLAT_MAX_PWR_LVL] = {0}; 103 104 for (cpu_idx = 0; cpu_idx < psci_plat_core_count; cpu_idx++) { 105 psci_get_parent_pwr_domain_nodes(cpu_idx, 106 PLAT_MAX_PWR_LVL, 107 temp_index); 108 for (j = (int)PLAT_MAX_PWR_LVL - 1; j >= 0; j--) { 109 if (temp_index[j] != nodes_idx[j]) { 110 nodes_idx[j] = temp_index[j]; 111 psci_non_cpu_pd_nodes[nodes_idx[j]].cpu_start_idx 112 = cpu_idx; 113 } 114 psci_non_cpu_pd_nodes[nodes_idx[j]].ncpus++; 115 } 116 } 117 } 118 119 static void __init populate_cpu_data(void) 120 { 121 for (unsigned int idx = 0; idx < psci_plat_core_count; idx++) { 122 set_cpu_data_by_index(idx, warmboot_ep_info, &warmboot_ep_info[idx]); 123 } 124 } 125 126 /******************************************************************************* 127 * Core routine to populate the power domain tree. The tree descriptor passed by 128 * the platform is populated breadth-first and the first entry in the map 129 * informs the number of root power domains. The parent nodes of the root nodes 130 * will point to an invalid entry(-1). 131 ******************************************************************************/ 132 static unsigned int __init populate_power_domain_tree(const unsigned char 133 *topology) 134 { 135 unsigned int i, j = 0U, num_nodes_at_lvl = 1U, num_nodes_at_next_lvl; 136 unsigned int node_index = 0U, num_children; 137 unsigned int parent_node_index = 0U; 138 int level = (int)PLAT_MAX_PWR_LVL; 139 140 /* 141 * For each level the inputs are: 142 * - number of nodes at this level in plat_array i.e. num_nodes_at_level 143 * This is the sum of values of nodes at the parent level. 144 * - Index of first entry at this level in the plat_array i.e. 145 * parent_node_index. 146 * - Index of first free entry in psci_non_cpu_pd_nodes[] or 147 * psci_cpu_pd_nodes[] i.e. node_index depending upon the level. 148 */ 149 while (level >= (int) PSCI_CPU_PWR_LVL) { 150 num_nodes_at_next_lvl = 0U; 151 /* 152 * For each entry (parent node) at this level in the plat_array: 153 * - Find the number of children 154 * - Allocate a node in a power domain array for each child 155 * - Set the parent of the child to the parent_node_index - 1 156 * - Increment parent_node_index to point to the next parent 157 * - Accumulate the number of children at next level. 158 */ 159 for (i = 0U; i < num_nodes_at_lvl; i++) { 160 assert(parent_node_index <= 161 PSCI_NUM_NON_CPU_PWR_DOMAINS); 162 num_children = topology[parent_node_index]; 163 164 for (j = node_index; 165 j < (node_index + num_children); j++) { 166 psci_init_pwr_domain_node((uint16_t)j, 167 parent_node_index - 1U, 168 (unsigned char)level); 169 } 170 node_index = j; 171 num_nodes_at_next_lvl += num_children; 172 parent_node_index++; 173 } 174 175 num_nodes_at_lvl = num_nodes_at_next_lvl; 176 level--; 177 178 /* Reset the index for the cpu power domain array */ 179 if (level == (int) PSCI_CPU_PWR_LVL) { 180 node_index = 0; 181 } 182 } 183 184 /* Validate the sanity of array exported by the platform */ 185 assert(j <= PLATFORM_CORE_COUNT); 186 return j; 187 } 188 189 /******************************************************************************* 190 * This function does the architectural setup and takes the warm boot 191 * entry-point `mailbox_ep` as an argument. The function also initializes the 192 * power domain topology tree by querying the platform. The power domain nodes 193 * higher than the CPU are populated in the array psci_non_cpu_pd_nodes[] and 194 * the CPU power domains are populated in psci_cpu_pd_nodes[]. The platform 195 * exports its static topology map through the 196 * populate_power_domain_topology_tree() API. The algorithm populates the 197 * psci_non_cpu_pd_nodes and psci_cpu_pd_nodes iteratively by using this 198 * topology map. On a platform that implements two clusters of 2 cpus each, 199 * and supporting 3 domain levels, the populated psci_non_cpu_pd_nodes would 200 * look like this: 201 * 202 * --------------------------------------------------- 203 * | system node | cluster 0 node | cluster 1 node | 204 * --------------------------------------------------- 205 * 206 * And populated psci_cpu_pd_nodes would look like this : 207 * <- cpus cluster0 -><- cpus cluster1 -> 208 * ------------------------------------------------ 209 * | CPU 0 | CPU 1 | CPU 2 | CPU 3 | 210 * ------------------------------------------------ 211 ******************************************************************************/ 212 int __init psci_setup(const psci_lib_args_t *lib_args) 213 { 214 const unsigned char *topology_tree; 215 unsigned int cpu_idx = plat_my_core_pos(); 216 217 assert(VERIFY_PSCI_LIB_ARGS_V1(lib_args)); 218 219 /* Do the Architectural initialization */ 220 psci_arch_setup(); 221 222 /* Query the topology map from the platform */ 223 topology_tree = plat_get_power_domain_tree_desc(); 224 225 /* Populate the power domain arrays using the platform topology map */ 226 psci_plat_core_count = populate_power_domain_tree(topology_tree); 227 228 /* Update the CPU limits for each node in psci_non_cpu_pd_nodes */ 229 psci_update_pwrlvl_limits(); 230 231 /* Initialise the warmboot entrypoints */ 232 populate_cpu_data(); 233 234 /* Populate the mpidr field of cpu node for this CPU */ 235 PER_CPU_BY_INDEX(psci_cpu_pd_nodes, cpu_idx)->mpidr = 236 read_mpidr() & MPIDR_AFFINITY_MASK; 237 238 psci_init_req_local_pwr_states(); 239 240 /* 241 * Set the requested and target state of this CPU and all the higher 242 * power domain levels for this CPU to run. 243 */ 244 psci_set_pwr_domains_to_run(cpu_idx, PLAT_MAX_PWR_LVL); 245 246 (void) plat_setup_psci_ops((uintptr_t)lib_args->mailbox_ep, 247 &psci_plat_pm_ops); 248 assert(psci_plat_pm_ops != NULL); 249 250 /* 251 * Flush `psci_plat_pm_ops` as it will be accessed by secondary CPUs 252 * during warm boot, possibly before data cache is enabled. 253 */ 254 psci_flush_dcache_range((uintptr_t)&psci_plat_pm_ops, 255 sizeof(psci_plat_pm_ops)); 256 257 /* Initialize the psci capability */ 258 psci_caps = PSCI_GENERIC_CAP; 259 260 if (psci_plat_pm_ops->pwr_domain_off != NULL) { 261 psci_caps |= define_psci_cap(PSCI_CPU_OFF); 262 } 263 if ((psci_plat_pm_ops->pwr_domain_on != NULL) && 264 (psci_plat_pm_ops->pwr_domain_on_finish != NULL)) { 265 psci_caps |= define_psci_cap(PSCI_CPU_ON_AARCH64); 266 } 267 if ((psci_plat_pm_ops->pwr_domain_suspend != NULL) && 268 (psci_plat_pm_ops->pwr_domain_suspend_finish != NULL)) { 269 if (psci_plat_pm_ops->validate_power_state != NULL) { 270 psci_caps |= define_psci_cap(PSCI_CPU_SUSPEND_AARCH64); 271 } 272 if (psci_plat_pm_ops->get_sys_suspend_power_state != NULL) { 273 psci_caps |= define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64); 274 } 275 #if PSCI_OS_INIT_MODE 276 psci_caps |= define_psci_cap(PSCI_SET_SUSPEND_MODE); 277 #endif 278 } 279 if (psci_plat_pm_ops->system_off != NULL) { 280 psci_caps |= define_psci_cap(PSCI_SYSTEM_OFF); 281 } 282 if (psci_plat_pm_ops->system_reset != NULL) { 283 psci_caps |= define_psci_cap(PSCI_SYSTEM_RESET); 284 } 285 if (psci_plat_pm_ops->get_node_hw_state != NULL) { 286 psci_caps |= define_psci_cap(PSCI_NODE_HW_STATE_AARCH64); 287 } 288 if ((psci_plat_pm_ops->read_mem_protect != NULL) && 289 (psci_plat_pm_ops->write_mem_protect != NULL)) { 290 psci_caps |= define_psci_cap(PSCI_MEM_PROTECT); 291 } 292 if (psci_plat_pm_ops->mem_protect_chk != NULL) { 293 psci_caps |= define_psci_cap(PSCI_MEM_CHK_RANGE_AARCH64); 294 } 295 if (psci_plat_pm_ops->system_reset2 != NULL) { 296 psci_caps |= define_psci_cap(PSCI_SYSTEM_RESET2_AARCH64); 297 } 298 #if ENABLE_PSCI_STAT 299 psci_caps |= define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64); 300 psci_caps |= define_psci_cap(PSCI_STAT_COUNT_AARCH64); 301 #endif 302 303 return 0; 304 } 305 306 /******************************************************************************* 307 * This duplicates what the primary cpu did after a cold boot in BL1. The same 308 * needs to be done when a cpu is hotplugged in. This function could also over- 309 * ride any EL3 setup done by BL1 as this code resides in rw memory. 310 ******************************************************************************/ 311 void psci_arch_setup(void) 312 { 313 #if (ARM_ARCH_MAJOR > 7) || defined(ARMV7_SUPPORTS_GENERIC_TIMER) 314 /* Program the counter frequency */ 315 write_cntfrq_el0(plat_get_syscnt_freq2()); 316 #endif 317 318 /* Initialize the cpu_ops pointer. */ 319 cpu_data_init_cpu_ops(); 320 321 /* Initialize the cached percpu ID register values */ 322 cm_init_percpu_once_regs(); 323 324 /* Having initialized cpu_ops, we can now print errata status */ 325 print_errata_status(); 326 327 } 328 329 /****************************************************************************** 330 * PSCI Library interface to initialize the cpu context for the next non 331 * secure image during cold boot. The relevant registers in the cpu context 332 * need to be retrieved and programmed on return from this interface. 333 *****************************************************************************/ 334 void psci_prepare_next_non_secure_ctx(entry_point_info_t *next_image_info) 335 { 336 assert(GET_SECURITY_STATE(next_image_info->h.attr) == NON_SECURE); 337 cm_init_my_context(next_image_info); 338 cm_prepare_el3_exit(NON_SECURE); 339 } 340