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