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 ******************************************************************************/
psci_init_pwr_domain_node(uint16_t node_idx,unsigned int parent_idx,unsigned char level)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 *******************************************************************************/
psci_update_pwrlvl_limits(void)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
populate_cpu_data(void)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 ******************************************************************************/
populate_power_domain_tree(const unsigned char * topology)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 ******************************************************************************/
psci_setup(const psci_lib_args_t * lib_args)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 ******************************************************************************/
psci_arch_setup(void)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 *****************************************************************************/
psci_prepare_next_non_secure_ctx(entry_point_info_t * next_image_info)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