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