Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 1 | /* |
Dan Handley | e83b0ca | 2014-01-14 18:17:09 +0000 | [diff] [blame] | 2 | * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 3 | * |
| 4 | * Redistribution and use in source and binary forms, with or without |
| 5 | * modification, are permitted provided that the following conditions are met: |
| 6 | * |
| 7 | * Redistributions of source code must retain the above copyright notice, this |
| 8 | * list of conditions and the following disclaimer. |
| 9 | * |
| 10 | * Redistributions in binary form must reproduce the above copyright notice, |
| 11 | * this list of conditions and the following disclaimer in the documentation |
| 12 | * and/or other materials provided with the distribution. |
| 13 | * |
| 14 | * Neither the name of ARM nor the names of its contributors may be used |
| 15 | * to endorse or promote products derived from this software without specific |
| 16 | * prior written permission. |
| 17 | * |
| 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 28 | * POSSIBILITY OF SUCH DAMAGE. |
| 29 | */ |
| 30 | |
Dan Handley | 2bd4ef2 | 2014-04-09 13:14:54 +0100 | [diff] [blame] | 31 | #include <arch.h> |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 32 | #include <arch_helpers.h> |
Dan Handley | 2bd4ef2 | 2014-04-09 13:14:54 +0100 | [diff] [blame] | 33 | #include <assert.h> |
| 34 | #include <bl_common.h> |
| 35 | #include <context.h> |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 36 | #include <context_mgmt.h> |
Dan Handley | 2bd4ef2 | 2014-04-09 13:14:54 +0100 | [diff] [blame] | 37 | #include <platform.h> |
| 38 | #include <stddef.h> |
Dan Handley | 714a0d2 | 2014-04-09 13:13:04 +0100 | [diff] [blame] | 39 | #include "psci_private.h" |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 40 | |
| 41 | /******************************************************************************* |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 42 | * Per cpu non-secure contexts used to program the architectural state prior |
| 43 | * return to the normal world. |
| 44 | * TODO: Use the memory allocator to set aside memory for the contexts instead |
| 45 | * of relying on platform defined constants. Using PSCI_NUM_AFFS will be an |
| 46 | * overkill. |
| 47 | ******************************************************************************/ |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 48 | static cpu_context_t psci_ns_context[PLATFORM_CORE_COUNT]; |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 49 | |
| 50 | /******************************************************************************* |
Dan Handley | 60b13e3 | 2014-05-14 15:13:16 +0100 | [diff] [blame] | 51 | * In a system, a certain number of affinity instances are present at an |
| 52 | * affinity level. The cumulative number of instances across all levels are |
| 53 | * stored in 'psci_aff_map'. The topology tree has been flattenned into this |
| 54 | * array. To retrieve nodes, information about the extents of each affinity |
| 55 | * level i.e. start index and end index needs to be present. 'psci_aff_limits' |
| 56 | * stores this information. |
| 57 | ******************************************************************************/ |
| 58 | static aff_limits_node_t psci_aff_limits[MPIDR_MAX_AFFLVL + 1]; |
| 59 | |
| 60 | /******************************************************************************* |
| 61 | * 'psci_ns_einfo_idx' keeps track of the next free index in the |
Andrew Thoelke | 4e12607 | 2014-06-04 21:10:52 +0100 | [diff] [blame^] | 62 | * 'psci_suspend_context' arrays. |
Dan Handley | 60b13e3 | 2014-05-14 15:13:16 +0100 | [diff] [blame] | 63 | ******************************************************************************/ |
| 64 | static unsigned int psci_ns_einfo_idx; |
| 65 | |
| 66 | /******************************************************************************* |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 67 | * Routines for retrieving the node corresponding to an affinity level instance |
| 68 | * in the mpidr. The first one uses binary search to find the node corresponding |
| 69 | * to the mpidr (key) at a particular affinity level. The second routine decides |
| 70 | * extents of the binary search at each affinity level. |
| 71 | ******************************************************************************/ |
| 72 | static int psci_aff_map_get_idx(unsigned long key, |
| 73 | int min_idx, |
| 74 | int max_idx) |
| 75 | { |
| 76 | int mid; |
| 77 | |
| 78 | /* |
| 79 | * Terminating condition: If the max and min indices have crossed paths |
| 80 | * during the binary search then the key has not been found. |
| 81 | */ |
| 82 | if (max_idx < min_idx) |
| 83 | return PSCI_E_INVALID_PARAMS; |
| 84 | |
| 85 | /* |
| 86 | * Bisect the array around 'mid' and then recurse into the array chunk |
| 87 | * where the key is likely to be found. The mpidrs in each node in the |
| 88 | * 'psci_aff_map' for a given affinity level are stored in an ascending |
| 89 | * order which makes the binary search possible. |
| 90 | */ |
| 91 | mid = min_idx + ((max_idx - min_idx) >> 1); /* Divide by 2 */ |
| 92 | if (psci_aff_map[mid].mpidr > key) |
| 93 | return psci_aff_map_get_idx(key, min_idx, mid - 1); |
| 94 | else if (psci_aff_map[mid].mpidr < key) |
| 95 | return psci_aff_map_get_idx(key, mid + 1, max_idx); |
| 96 | else |
| 97 | return mid; |
| 98 | } |
| 99 | |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 100 | aff_map_node_t *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl) |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 101 | { |
| 102 | int rc; |
| 103 | |
| 104 | /* Right shift the mpidr to the required affinity level */ |
| 105 | mpidr = mpidr_mask_lower_afflvls(mpidr, aff_lvl); |
| 106 | |
| 107 | rc = psci_aff_map_get_idx(mpidr, |
| 108 | psci_aff_limits[aff_lvl].min, |
| 109 | psci_aff_limits[aff_lvl].max); |
| 110 | if (rc >= 0) |
| 111 | return &psci_aff_map[rc]; |
| 112 | else |
| 113 | return NULL; |
| 114 | } |
| 115 | |
| 116 | /******************************************************************************* |
Achin Gupta | 0959db5 | 2013-12-02 17:33:04 +0000 | [diff] [blame] | 117 | * This function populates an array with nodes corresponding to a given range of |
| 118 | * affinity levels in an mpidr. It returns successfully only when the affinity |
| 119 | * levels are correct, the mpidr is valid i.e. no affinity level is absent from |
| 120 | * the topology tree & the affinity instance at level 0 is not absent. |
| 121 | ******************************************************************************/ |
| 122 | int psci_get_aff_map_nodes(unsigned long mpidr, |
| 123 | int start_afflvl, |
| 124 | int end_afflvl, |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 125 | mpidr_aff_map_nodes_t mpidr_nodes) |
Achin Gupta | 0959db5 | 2013-12-02 17:33:04 +0000 | [diff] [blame] | 126 | { |
| 127 | int rc = PSCI_E_INVALID_PARAMS, level; |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 128 | aff_map_node_t *node; |
Achin Gupta | 0959db5 | 2013-12-02 17:33:04 +0000 | [diff] [blame] | 129 | |
| 130 | rc = psci_check_afflvl_range(start_afflvl, end_afflvl); |
| 131 | if (rc != PSCI_E_SUCCESS) |
| 132 | return rc; |
| 133 | |
| 134 | for (level = start_afflvl; level <= end_afflvl; level++) { |
| 135 | |
| 136 | /* |
| 137 | * Grab the node for each affinity level. No affinity level |
| 138 | * can be missing as that would mean that the topology tree |
| 139 | * is corrupted. |
| 140 | */ |
| 141 | node = psci_get_aff_map_node(mpidr, level); |
| 142 | if (node == NULL) { |
| 143 | rc = PSCI_E_INVALID_PARAMS; |
| 144 | break; |
| 145 | } |
| 146 | |
| 147 | /* |
| 148 | * Skip absent affinity levels unless it's afffinity level 0. |
| 149 | * An absent cpu means that the mpidr is invalid. Save the |
| 150 | * pointer to the node for the present affinity level |
| 151 | */ |
| 152 | if (!(node->state & PSCI_AFF_PRESENT)) { |
| 153 | if (level == MPIDR_AFFLVL0) { |
| 154 | rc = PSCI_E_INVALID_PARAMS; |
| 155 | break; |
| 156 | } |
| 157 | |
| 158 | mpidr_nodes[level] = NULL; |
| 159 | } else |
| 160 | mpidr_nodes[level] = node; |
| 161 | } |
| 162 | |
| 163 | return rc; |
| 164 | } |
| 165 | |
| 166 | /******************************************************************************* |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 167 | * Function which initializes the 'aff_map_node' corresponding to an affinity |
| 168 | * level instance. Each node has a unique mpidr, level and bakery lock. The data |
| 169 | * field is opaque and holds affinity level specific data e.g. for affinity |
| 170 | * level 0 it contains the index into arrays that hold the secure/non-secure |
| 171 | * state for a cpu that's been turned on/off |
| 172 | ******************************************************************************/ |
| 173 | static void psci_init_aff_map_node(unsigned long mpidr, |
| 174 | int level, |
| 175 | unsigned int idx) |
| 176 | { |
| 177 | unsigned char state; |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 178 | uint32_t linear_id; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 179 | psci_aff_map[idx].mpidr = mpidr; |
| 180 | psci_aff_map[idx].level = level; |
| 181 | bakery_lock_init(&psci_aff_map[idx].lock); |
| 182 | |
| 183 | /* |
| 184 | * If an affinity instance is present then mark it as OFF to begin with. |
| 185 | */ |
| 186 | state = plat_get_aff_state(level, mpidr); |
| 187 | psci_aff_map[idx].state = state; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 188 | |
| 189 | if (level == MPIDR_AFFLVL0) { |
Achin Gupta | 75f7367 | 2013-12-05 16:33:10 +0000 | [diff] [blame] | 190 | |
| 191 | /* |
| 192 | * Mark the cpu as OFF. Higher affinity level reference counts |
| 193 | * have already been memset to 0 |
| 194 | */ |
| 195 | if (state & PSCI_AFF_PRESENT) |
| 196 | psci_set_state(&psci_aff_map[idx], PSCI_STATE_OFF); |
| 197 | |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 198 | /* Ensure that we have not overflowed the psci_ns_einfo array */ |
| 199 | assert(psci_ns_einfo_idx < PSCI_NUM_AFFS); |
| 200 | |
| 201 | psci_aff_map[idx].data = psci_ns_einfo_idx; |
Vikram Kanigiri | f100f41 | 2014-04-01 19:26:26 +0100 | [diff] [blame] | 202 | /* Invalidate the suspend context for the node */ |
| 203 | psci_suspend_context[psci_ns_einfo_idx].power_state = PSCI_INVALID_DATA; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 204 | psci_ns_einfo_idx++; |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 205 | |
| 206 | /* |
| 207 | * Associate a non-secure context with this affinity |
| 208 | * instance through the context management library. |
| 209 | */ |
| 210 | linear_id = platform_get_core_pos(mpidr); |
| 211 | assert(linear_id < PLATFORM_CORE_COUNT); |
| 212 | |
Andrew Thoelke | a2f6553 | 2014-05-14 17:09:32 +0100 | [diff] [blame] | 213 | cm_set_context_by_mpidr(mpidr, |
| 214 | (void *) &psci_ns_context[linear_id], |
| 215 | NON_SECURE); |
Jeenu Viswambharan | caa8493 | 2014-02-06 10:36:15 +0000 | [diff] [blame] | 216 | |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 217 | } |
| 218 | |
| 219 | return; |
| 220 | } |
| 221 | |
| 222 | /******************************************************************************* |
| 223 | * Core routine used by the Breadth-First-Search algorithm to populate the |
| 224 | * affinity tree. Each level in the tree corresponds to an affinity level. This |
| 225 | * routine's aim is to traverse to the target affinity level and populate nodes |
| 226 | * in the 'psci_aff_map' for all the siblings at that level. It uses the current |
| 227 | * affinity level to keep track of how many levels from the root of the tree |
| 228 | * have been traversed. If the current affinity level != target affinity level, |
| 229 | * then the platform is asked to return the number of children that each |
| 230 | * affinity instance has at the current affinity level. Traversal is then done |
| 231 | * for each child at the next lower level i.e. current affinity level - 1. |
| 232 | * |
| 233 | * CAUTION: This routine assumes that affinity instance ids are allocated in a |
| 234 | * monotonically increasing manner at each affinity level in a mpidr starting |
| 235 | * from 0. If the platform breaks this assumption then this code will have to |
| 236 | * be reworked accordingly. |
| 237 | ******************************************************************************/ |
| 238 | static unsigned int psci_init_aff_map(unsigned long mpidr, |
| 239 | unsigned int affmap_idx, |
| 240 | int cur_afflvl, |
| 241 | int tgt_afflvl) |
| 242 | { |
| 243 | unsigned int ctr, aff_count; |
| 244 | |
| 245 | assert(cur_afflvl >= tgt_afflvl); |
| 246 | |
| 247 | /* |
| 248 | * Find the number of siblings at the current affinity level & |
| 249 | * assert if there are none 'cause then we have been invoked with |
| 250 | * an invalid mpidr. |
| 251 | */ |
| 252 | aff_count = plat_get_aff_count(cur_afflvl, mpidr); |
| 253 | assert(aff_count); |
| 254 | |
| 255 | if (tgt_afflvl < cur_afflvl) { |
| 256 | for (ctr = 0; ctr < aff_count; ctr++) { |
| 257 | mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); |
| 258 | affmap_idx = psci_init_aff_map(mpidr, |
| 259 | affmap_idx, |
| 260 | cur_afflvl - 1, |
| 261 | tgt_afflvl); |
| 262 | } |
| 263 | } else { |
| 264 | for (ctr = 0; ctr < aff_count; ctr++, affmap_idx++) { |
| 265 | mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); |
| 266 | psci_init_aff_map_node(mpidr, cur_afflvl, affmap_idx); |
| 267 | } |
| 268 | |
| 269 | /* affmap_idx is 1 greater than the max index of cur_afflvl */ |
| 270 | psci_aff_limits[cur_afflvl].max = affmap_idx - 1; |
| 271 | } |
| 272 | |
| 273 | return affmap_idx; |
| 274 | } |
| 275 | |
| 276 | /******************************************************************************* |
| 277 | * This function initializes the topology tree by querying the platform. To do |
| 278 | * so, it's helper routines implement a Breadth-First-Search. At each affinity |
| 279 | * level the platform conveys the number of affinity instances that exist i.e. |
| 280 | * the affinity count. The algorithm populates the psci_aff_map recursively |
| 281 | * using this information. On a platform that implements two clusters of 4 cpus |
| 282 | * each, the populated aff_map_array would look like this: |
| 283 | * |
| 284 | * <- cpus cluster0 -><- cpus cluster1 -> |
| 285 | * --------------------------------------------------- |
| 286 | * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 | |
| 287 | * --------------------------------------------------- |
| 288 | * ^ ^ |
| 289 | * cluster __| cpu __| |
| 290 | * limit limit |
| 291 | * |
| 292 | * The first 2 entries are of the cluster nodes. The next 4 entries are of cpus |
| 293 | * within cluster 0. The last 4 entries are of cpus within cluster 1. |
| 294 | * The 'psci_aff_limits' array contains the max & min index of each affinity |
| 295 | * level within the 'psci_aff_map' array. This allows restricting search of a |
| 296 | * node at an affinity level between the indices in the limits array. |
| 297 | ******************************************************************************/ |
Achin Gupta | 7421b46 | 2014-02-01 18:53:26 +0000 | [diff] [blame] | 298 | int32_t psci_setup(void) |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 299 | { |
Achin Gupta | 7421b46 | 2014-02-01 18:53:26 +0000 | [diff] [blame] | 300 | unsigned long mpidr = read_mpidr(); |
James Morrissey | 40a6f64 | 2014-02-10 14:24:36 +0000 | [diff] [blame] | 301 | int afflvl, affmap_idx, max_afflvl; |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 302 | aff_map_node_t *node; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 303 | |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 304 | psci_ns_einfo_idx = 0; |
| 305 | psci_plat_pm_ops = NULL; |
| 306 | |
| 307 | /* Find out the maximum affinity level that the platform implements */ |
| 308 | max_afflvl = get_max_afflvl(); |
| 309 | assert(max_afflvl <= MPIDR_MAX_AFFLVL); |
| 310 | |
| 311 | /* |
| 312 | * This call traverses the topology tree with help from the platform and |
| 313 | * populates the affinity map using a breadth-first-search recursively. |
| 314 | * We assume that the platform allocates affinity instance ids from 0 |
| 315 | * onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0 |
| 316 | */ |
| 317 | affmap_idx = 0; |
| 318 | for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) { |
| 319 | affmap_idx = psci_init_aff_map(FIRST_MPIDR, |
| 320 | affmap_idx, |
| 321 | max_afflvl, |
| 322 | afflvl); |
| 323 | } |
| 324 | |
| 325 | /* |
| 326 | * Set the bounds for the affinity counts of each level in the map. Also |
| 327 | * flush out the entire array so that it's visible to subsequent power |
| 328 | * management operations. The 'psci_aff_map' array is allocated in |
| 329 | * coherent memory so does not need flushing. The 'psci_aff_limits' |
| 330 | * array is allocated in normal memory. It will be accessed when the mmu |
| 331 | * is off e.g. after reset. Hence it needs to be flushed. |
| 332 | */ |
| 333 | for (afflvl = MPIDR_AFFLVL0; afflvl < max_afflvl; afflvl++) { |
| 334 | psci_aff_limits[afflvl].min = |
| 335 | psci_aff_limits[afflvl + 1].max + 1; |
| 336 | } |
| 337 | |
| 338 | flush_dcache_range((unsigned long) psci_aff_limits, |
| 339 | sizeof(psci_aff_limits)); |
| 340 | |
| 341 | /* |
| 342 | * Mark the affinity instances in our mpidr as ON. No need to lock as |
| 343 | * this is the primary cpu. |
| 344 | */ |
| 345 | mpidr &= MPIDR_AFFINITY_MASK; |
Achin Gupta | 75f7367 | 2013-12-05 16:33:10 +0000 | [diff] [blame] | 346 | for (afflvl = MPIDR_AFFLVL0; afflvl <= max_afflvl; afflvl++) { |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 347 | |
| 348 | node = psci_get_aff_map_node(mpidr, afflvl); |
| 349 | assert(node); |
| 350 | |
| 351 | /* Mark each present node as ON. */ |
Achin Gupta | 75f7367 | 2013-12-05 16:33:10 +0000 | [diff] [blame] | 352 | if (node->state & PSCI_AFF_PRESENT) |
| 353 | psci_set_state(node, PSCI_STATE_ON); |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 354 | } |
| 355 | |
James Morrissey | 40a6f64 | 2014-02-10 14:24:36 +0000 | [diff] [blame] | 356 | platform_setup_pm(&psci_plat_pm_ops); |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 357 | assert(psci_plat_pm_ops); |
| 358 | |
Achin Gupta | 7421b46 | 2014-02-01 18:53:26 +0000 | [diff] [blame] | 359 | return 0; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 360 | } |