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 | /******************************************************************************* |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 61 | * Routines for retrieving the node corresponding to an affinity level instance |
| 62 | * in the mpidr. The first one uses binary search to find the node corresponding |
| 63 | * to the mpidr (key) at a particular affinity level. The second routine decides |
| 64 | * extents of the binary search at each affinity level. |
| 65 | ******************************************************************************/ |
| 66 | static int psci_aff_map_get_idx(unsigned long key, |
| 67 | int min_idx, |
| 68 | int max_idx) |
| 69 | { |
| 70 | int mid; |
| 71 | |
| 72 | /* |
| 73 | * Terminating condition: If the max and min indices have crossed paths |
| 74 | * during the binary search then the key has not been found. |
| 75 | */ |
| 76 | if (max_idx < min_idx) |
| 77 | return PSCI_E_INVALID_PARAMS; |
| 78 | |
| 79 | /* |
Soby Mathew | 2b69750 | 2014-10-02 17:24:19 +0100 | [diff] [blame] | 80 | * Make sure we are within array limits. |
| 81 | */ |
| 82 | assert(min_idx >= 0 && max_idx < PSCI_NUM_AFFS); |
| 83 | |
| 84 | /* |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 85 | * Bisect the array around 'mid' and then recurse into the array chunk |
| 86 | * where the key is likely to be found. The mpidrs in each node in the |
| 87 | * 'psci_aff_map' for a given affinity level are stored in an ascending |
| 88 | * order which makes the binary search possible. |
| 89 | */ |
| 90 | mid = min_idx + ((max_idx - min_idx) >> 1); /* Divide by 2 */ |
Soby Mathew | 2b69750 | 2014-10-02 17:24:19 +0100 | [diff] [blame] | 91 | |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 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 | |
Soby Mathew | 2b69750 | 2014-10-02 17:24:19 +0100 | [diff] [blame] | 104 | if (aff_lvl > get_max_afflvl()) |
| 105 | return NULL; |
| 106 | |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 107 | /* Right shift the mpidr to the required affinity level */ |
| 108 | mpidr = mpidr_mask_lower_afflvls(mpidr, aff_lvl); |
| 109 | |
| 110 | rc = psci_aff_map_get_idx(mpidr, |
| 111 | psci_aff_limits[aff_lvl].min, |
| 112 | psci_aff_limits[aff_lvl].max); |
| 113 | if (rc >= 0) |
| 114 | return &psci_aff_map[rc]; |
| 115 | else |
| 116 | return NULL; |
| 117 | } |
| 118 | |
| 119 | /******************************************************************************* |
Achin Gupta | 0959db5 | 2013-12-02 17:33:04 +0000 | [diff] [blame] | 120 | * This function populates an array with nodes corresponding to a given range of |
| 121 | * affinity levels in an mpidr. It returns successfully only when the affinity |
| 122 | * levels are correct, the mpidr is valid i.e. no affinity level is absent from |
| 123 | * the topology tree & the affinity instance at level 0 is not absent. |
| 124 | ******************************************************************************/ |
| 125 | int psci_get_aff_map_nodes(unsigned long mpidr, |
| 126 | int start_afflvl, |
| 127 | int end_afflvl, |
Achin Gupta | 56bcdc2 | 2014-07-28 00:15:23 +0100 | [diff] [blame] | 128 | aff_map_node_t *mpidr_nodes[]) |
Achin Gupta | 0959db5 | 2013-12-02 17:33:04 +0000 | [diff] [blame] | 129 | { |
| 130 | int rc = PSCI_E_INVALID_PARAMS, level; |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 131 | aff_map_node_t *node; |
Achin Gupta | 0959db5 | 2013-12-02 17:33:04 +0000 | [diff] [blame] | 132 | |
| 133 | rc = psci_check_afflvl_range(start_afflvl, end_afflvl); |
| 134 | if (rc != PSCI_E_SUCCESS) |
| 135 | return rc; |
| 136 | |
| 137 | for (level = start_afflvl; level <= end_afflvl; level++) { |
| 138 | |
| 139 | /* |
| 140 | * Grab the node for each affinity level. No affinity level |
| 141 | * can be missing as that would mean that the topology tree |
| 142 | * is corrupted. |
| 143 | */ |
| 144 | node = psci_get_aff_map_node(mpidr, level); |
| 145 | if (node == NULL) { |
| 146 | rc = PSCI_E_INVALID_PARAMS; |
| 147 | break; |
| 148 | } |
| 149 | |
| 150 | /* |
| 151 | * Skip absent affinity levels unless it's afffinity level 0. |
| 152 | * An absent cpu means that the mpidr is invalid. Save the |
| 153 | * pointer to the node for the present affinity level |
| 154 | */ |
| 155 | if (!(node->state & PSCI_AFF_PRESENT)) { |
| 156 | if (level == MPIDR_AFFLVL0) { |
| 157 | rc = PSCI_E_INVALID_PARAMS; |
| 158 | break; |
| 159 | } |
| 160 | |
| 161 | mpidr_nodes[level] = NULL; |
| 162 | } else |
| 163 | mpidr_nodes[level] = node; |
| 164 | } |
| 165 | |
| 166 | return rc; |
| 167 | } |
| 168 | |
| 169 | /******************************************************************************* |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 170 | * Function which initializes the 'aff_map_node' corresponding to an affinity |
| 171 | * level instance. Each node has a unique mpidr, level and bakery lock. The data |
| 172 | * field is opaque and holds affinity level specific data e.g. for affinity |
| 173 | * level 0 it contains the index into arrays that hold the secure/non-secure |
| 174 | * state for a cpu that's been turned on/off |
| 175 | ******************************************************************************/ |
| 176 | static void psci_init_aff_map_node(unsigned long mpidr, |
| 177 | int level, |
| 178 | unsigned int idx) |
| 179 | { |
| 180 | unsigned char state; |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 181 | uint32_t linear_id; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 182 | psci_aff_map[idx].mpidr = mpidr; |
| 183 | psci_aff_map[idx].level = level; |
| 184 | bakery_lock_init(&psci_aff_map[idx].lock); |
| 185 | |
| 186 | /* |
| 187 | * If an affinity instance is present then mark it as OFF to begin with. |
| 188 | */ |
| 189 | state = plat_get_aff_state(level, mpidr); |
| 190 | psci_aff_map[idx].state = state; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 191 | |
| 192 | if (level == MPIDR_AFFLVL0) { |
Achin Gupta | 75f7367 | 2013-12-05 16:33:10 +0000 | [diff] [blame] | 193 | |
| 194 | /* |
| 195 | * Mark the cpu as OFF. Higher affinity level reference counts |
| 196 | * have already been memset to 0 |
| 197 | */ |
| 198 | if (state & PSCI_AFF_PRESENT) |
| 199 | psci_set_state(&psci_aff_map[idx], PSCI_STATE_OFF); |
| 200 | |
Achin Gupta | ef7a28c | 2014-02-01 08:59:56 +0000 | [diff] [blame] | 201 | /* |
| 202 | * Associate a non-secure context with this affinity |
| 203 | * instance through the context management library. |
| 204 | */ |
| 205 | linear_id = platform_get_core_pos(mpidr); |
| 206 | assert(linear_id < PLATFORM_CORE_COUNT); |
| 207 | |
Achin Gupta | f3ccbab | 2014-07-25 14:52:47 +0100 | [diff] [blame] | 208 | /* Invalidate the suspend context for the node */ |
| 209 | set_cpu_data_by_index(linear_id, |
| 210 | psci_svc_cpu_data.power_state, |
| 211 | PSCI_INVALID_DATA); |
| 212 | |
Achin Gupta | f6b9e99 | 2014-07-31 11:19:11 +0100 | [diff] [blame] | 213 | /* |
| 214 | * There is no state associated with the current execution |
| 215 | * context so ensure that any reads of the highest affinity |
| 216 | * level in a powered down state return PSCI_INVALID_DATA. |
| 217 | */ |
| 218 | set_cpu_data_by_index(linear_id, |
| 219 | psci_svc_cpu_data.max_phys_off_afflvl, |
| 220 | PSCI_INVALID_DATA); |
| 221 | |
Soby Mathew | 7d861ea | 2014-11-18 10:14:14 +0000 | [diff] [blame] | 222 | flush_cpu_data_by_index(linear_id, psci_svc_cpu_data); |
| 223 | |
Andrew Thoelke | a2f6553 | 2014-05-14 17:09:32 +0100 | [diff] [blame] | 224 | cm_set_context_by_mpidr(mpidr, |
| 225 | (void *) &psci_ns_context[linear_id], |
| 226 | NON_SECURE); |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 227 | } |
| 228 | |
| 229 | return; |
| 230 | } |
| 231 | |
| 232 | /******************************************************************************* |
| 233 | * Core routine used by the Breadth-First-Search algorithm to populate the |
| 234 | * affinity tree. Each level in the tree corresponds to an affinity level. This |
| 235 | * routine's aim is to traverse to the target affinity level and populate nodes |
| 236 | * in the 'psci_aff_map' for all the siblings at that level. It uses the current |
| 237 | * affinity level to keep track of how many levels from the root of the tree |
| 238 | * have been traversed. If the current affinity level != target affinity level, |
| 239 | * then the platform is asked to return the number of children that each |
| 240 | * affinity instance has at the current affinity level. Traversal is then done |
| 241 | * for each child at the next lower level i.e. current affinity level - 1. |
| 242 | * |
| 243 | * CAUTION: This routine assumes that affinity instance ids are allocated in a |
| 244 | * monotonically increasing manner at each affinity level in a mpidr starting |
| 245 | * from 0. If the platform breaks this assumption then this code will have to |
| 246 | * be reworked accordingly. |
| 247 | ******************************************************************************/ |
| 248 | static unsigned int psci_init_aff_map(unsigned long mpidr, |
| 249 | unsigned int affmap_idx, |
| 250 | int cur_afflvl, |
| 251 | int tgt_afflvl) |
| 252 | { |
| 253 | unsigned int ctr, aff_count; |
| 254 | |
| 255 | assert(cur_afflvl >= tgt_afflvl); |
| 256 | |
| 257 | /* |
| 258 | * Find the number of siblings at the current affinity level & |
| 259 | * assert if there are none 'cause then we have been invoked with |
| 260 | * an invalid mpidr. |
| 261 | */ |
| 262 | aff_count = plat_get_aff_count(cur_afflvl, mpidr); |
| 263 | assert(aff_count); |
| 264 | |
| 265 | if (tgt_afflvl < cur_afflvl) { |
| 266 | for (ctr = 0; ctr < aff_count; ctr++) { |
| 267 | mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); |
| 268 | affmap_idx = psci_init_aff_map(mpidr, |
| 269 | affmap_idx, |
| 270 | cur_afflvl - 1, |
| 271 | tgt_afflvl); |
| 272 | } |
| 273 | } else { |
| 274 | for (ctr = 0; ctr < aff_count; ctr++, affmap_idx++) { |
| 275 | mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); |
| 276 | psci_init_aff_map_node(mpidr, cur_afflvl, affmap_idx); |
| 277 | } |
| 278 | |
| 279 | /* affmap_idx is 1 greater than the max index of cur_afflvl */ |
| 280 | psci_aff_limits[cur_afflvl].max = affmap_idx - 1; |
| 281 | } |
| 282 | |
| 283 | return affmap_idx; |
| 284 | } |
| 285 | |
| 286 | /******************************************************************************* |
| 287 | * This function initializes the topology tree by querying the platform. To do |
| 288 | * so, it's helper routines implement a Breadth-First-Search. At each affinity |
| 289 | * level the platform conveys the number of affinity instances that exist i.e. |
| 290 | * the affinity count. The algorithm populates the psci_aff_map recursively |
| 291 | * using this information. On a platform that implements two clusters of 4 cpus |
| 292 | * each, the populated aff_map_array would look like this: |
| 293 | * |
| 294 | * <- cpus cluster0 -><- cpus cluster1 -> |
| 295 | * --------------------------------------------------- |
| 296 | * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 | |
| 297 | * --------------------------------------------------- |
| 298 | * ^ ^ |
| 299 | * cluster __| cpu __| |
| 300 | * limit limit |
| 301 | * |
| 302 | * The first 2 entries are of the cluster nodes. The next 4 entries are of cpus |
| 303 | * within cluster 0. The last 4 entries are of cpus within cluster 1. |
| 304 | * The 'psci_aff_limits' array contains the max & min index of each affinity |
| 305 | * level within the 'psci_aff_map' array. This allows restricting search of a |
| 306 | * node at an affinity level between the indices in the limits array. |
| 307 | ******************************************************************************/ |
Achin Gupta | 7421b46 | 2014-02-01 18:53:26 +0000 | [diff] [blame] | 308 | int32_t psci_setup(void) |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 309 | { |
Achin Gupta | 7421b46 | 2014-02-01 18:53:26 +0000 | [diff] [blame] | 310 | unsigned long mpidr = read_mpidr(); |
James Morrissey | 40a6f64 | 2014-02-10 14:24:36 +0000 | [diff] [blame] | 311 | int afflvl, affmap_idx, max_afflvl; |
Dan Handley | e2712bc | 2014-04-10 15:37:22 +0100 | [diff] [blame] | 312 | aff_map_node_t *node; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 313 | |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 314 | psci_plat_pm_ops = NULL; |
| 315 | |
| 316 | /* Find out the maximum affinity level that the platform implements */ |
| 317 | max_afflvl = get_max_afflvl(); |
| 318 | assert(max_afflvl <= MPIDR_MAX_AFFLVL); |
| 319 | |
| 320 | /* |
| 321 | * This call traverses the topology tree with help from the platform and |
| 322 | * populates the affinity map using a breadth-first-search recursively. |
| 323 | * We assume that the platform allocates affinity instance ids from 0 |
| 324 | * onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0 |
| 325 | */ |
| 326 | affmap_idx = 0; |
| 327 | for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) { |
| 328 | affmap_idx = psci_init_aff_map(FIRST_MPIDR, |
| 329 | affmap_idx, |
| 330 | max_afflvl, |
| 331 | afflvl); |
| 332 | } |
| 333 | |
| 334 | /* |
| 335 | * Set the bounds for the affinity counts of each level in the map. Also |
| 336 | * flush out the entire array so that it's visible to subsequent power |
| 337 | * management operations. The 'psci_aff_map' array is allocated in |
| 338 | * coherent memory so does not need flushing. The 'psci_aff_limits' |
| 339 | * array is allocated in normal memory. It will be accessed when the mmu |
| 340 | * is off e.g. after reset. Hence it needs to be flushed. |
| 341 | */ |
| 342 | for (afflvl = MPIDR_AFFLVL0; afflvl < max_afflvl; afflvl++) { |
| 343 | psci_aff_limits[afflvl].min = |
| 344 | psci_aff_limits[afflvl + 1].max + 1; |
| 345 | } |
| 346 | |
| 347 | flush_dcache_range((unsigned long) psci_aff_limits, |
| 348 | sizeof(psci_aff_limits)); |
| 349 | |
| 350 | /* |
| 351 | * Mark the affinity instances in our mpidr as ON. No need to lock as |
| 352 | * this is the primary cpu. |
| 353 | */ |
| 354 | mpidr &= MPIDR_AFFINITY_MASK; |
Achin Gupta | 75f7367 | 2013-12-05 16:33:10 +0000 | [diff] [blame] | 355 | for (afflvl = MPIDR_AFFLVL0; afflvl <= max_afflvl; afflvl++) { |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 356 | |
| 357 | node = psci_get_aff_map_node(mpidr, afflvl); |
| 358 | assert(node); |
| 359 | |
| 360 | /* Mark each present node as ON. */ |
Achin Gupta | 75f7367 | 2013-12-05 16:33:10 +0000 | [diff] [blame] | 361 | if (node->state & PSCI_AFF_PRESENT) |
| 362 | psci_set_state(node, PSCI_STATE_ON); |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 363 | } |
| 364 | |
James Morrissey | 40a6f64 | 2014-02-10 14:24:36 +0000 | [diff] [blame] | 365 | platform_setup_pm(&psci_plat_pm_ops); |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 366 | assert(psci_plat_pm_ops); |
| 367 | |
Achin Gupta | 7421b46 | 2014-02-01 18:53:26 +0000 | [diff] [blame] | 368 | return 0; |
Achin Gupta | 4f6ad66 | 2013-10-25 09:08:21 +0100 | [diff] [blame] | 369 | } |