diff options
Diffstat (limited to 'common/psci/psci_setup.c')
| -rw-r--r-- | common/psci/psci_setup.c | 268 |
1 files changed, 268 insertions, 0 deletions
diff --git a/common/psci/psci_setup.c b/common/psci/psci_setup.c new file mode 100644 index 00000000..9095e75d --- /dev/null +++ b/common/psci/psci_setup.c @@ -0,0 +1,268 @@ +/* + * Copyright (c) 2013, ARM Limited. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <stdio.h> +#include <string.h> +#include <assert.h> +#include <arch_helpers.h> +#include <console.h> +#include <platform.h> +#include <psci_private.h> + +/******************************************************************************* + * Routines for retrieving the node corresponding to an affinity level instance + * in the mpidr. The first one uses binary search to find the node corresponding + * to the mpidr (key) at a particular affinity level. The second routine decides + * extents of the binary search at each affinity level. + ******************************************************************************/ +static int psci_aff_map_get_idx(unsigned long key, + int min_idx, + int max_idx) +{ + int mid; + + /* + * Terminating condition: If the max and min indices have crossed paths + * during the binary search then the key has not been found. + */ + if (max_idx < min_idx) + return PSCI_E_INVALID_PARAMS; + + /* + * Bisect the array around 'mid' and then recurse into the array chunk + * where the key is likely to be found. The mpidrs in each node in the + * 'psci_aff_map' for a given affinity level are stored in an ascending + * order which makes the binary search possible. + */ + mid = min_idx + ((max_idx - min_idx) >> 1); /* Divide by 2 */ + if (psci_aff_map[mid].mpidr > key) + return psci_aff_map_get_idx(key, min_idx, mid - 1); + else if (psci_aff_map[mid].mpidr < key) + return psci_aff_map_get_idx(key, mid + 1, max_idx); + else + return mid; +} + +aff_map_node *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl) +{ + int rc; + + /* Right shift the mpidr to the required affinity level */ + mpidr = mpidr_mask_lower_afflvls(mpidr, aff_lvl); + + rc = psci_aff_map_get_idx(mpidr, + psci_aff_limits[aff_lvl].min, + psci_aff_limits[aff_lvl].max); + if (rc >= 0) + return &psci_aff_map[rc]; + else + return NULL; +} + +/******************************************************************************* + * Function which initializes the 'aff_map_node' corresponding to an affinity + * level instance. Each node has a unique mpidr, level and bakery lock. The data + * field is opaque and holds affinity level specific data e.g. for affinity + * level 0 it contains the index into arrays that hold the secure/non-secure + * state for a cpu that's been turned on/off + ******************************************************************************/ +static void psci_init_aff_map_node(unsigned long mpidr, + int level, + unsigned int idx) +{ + unsigned char state; + psci_aff_map[idx].mpidr = mpidr; + psci_aff_map[idx].level = level; + bakery_lock_init(&psci_aff_map[idx].lock); + + /* + * If an affinity instance is present then mark it as OFF to begin with. + */ + state = plat_get_aff_state(level, mpidr); + psci_aff_map[idx].state = state; + if (state & PSCI_AFF_PRESENT) { + psci_set_state(psci_aff_map[idx].state, PSCI_STATE_OFF); + } + + if (level == MPIDR_AFFLVL0) { + /* Ensure that we have not overflowed the psci_ns_einfo array */ + assert(psci_ns_einfo_idx < PSCI_NUM_AFFS); + + psci_aff_map[idx].data = psci_ns_einfo_idx; + psci_ns_einfo_idx++; + } + + return; +} + +/******************************************************************************* + * Core routine used by the Breadth-First-Search algorithm to populate the + * affinity tree. Each level in the tree corresponds to an affinity level. This + * routine's aim is to traverse to the target affinity level and populate nodes + * in the 'psci_aff_map' for all the siblings at that level. It uses the current + * affinity level to keep track of how many levels from the root of the tree + * have been traversed. If the current affinity level != target affinity level, + * then the platform is asked to return the number of children that each + * affinity instance has at the current affinity level. Traversal is then done + * for each child at the next lower level i.e. current affinity level - 1. + * + * CAUTION: This routine assumes that affinity instance ids are allocated in a + * monotonically increasing manner at each affinity level in a mpidr starting + * from 0. If the platform breaks this assumption then this code will have to + * be reworked accordingly. + ******************************************************************************/ +static unsigned int psci_init_aff_map(unsigned long mpidr, + unsigned int affmap_idx, + int cur_afflvl, + int tgt_afflvl) +{ + unsigned int ctr, aff_count; + + assert(cur_afflvl >= tgt_afflvl); + + /* + * Find the number of siblings at the current affinity level & + * assert if there are none 'cause then we have been invoked with + * an invalid mpidr. + */ + aff_count = plat_get_aff_count(cur_afflvl, mpidr); + assert(aff_count); + + if (tgt_afflvl < cur_afflvl) { + for (ctr = 0; ctr < aff_count; ctr++) { + mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); + affmap_idx = psci_init_aff_map(mpidr, + affmap_idx, + cur_afflvl - 1, + tgt_afflvl); + } + } else { + for (ctr = 0; ctr < aff_count; ctr++, affmap_idx++) { + mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); + psci_init_aff_map_node(mpidr, cur_afflvl, affmap_idx); + } + + /* affmap_idx is 1 greater than the max index of cur_afflvl */ + psci_aff_limits[cur_afflvl].max = affmap_idx - 1; + } + + return affmap_idx; +} + +/******************************************************************************* + * This function initializes the topology tree by querying the platform. To do + * so, it's helper routines implement a Breadth-First-Search. At each affinity + * level the platform conveys the number of affinity instances that exist i.e. + * the affinity count. The algorithm populates the psci_aff_map recursively + * using this information. On a platform that implements two clusters of 4 cpus + * each, the populated aff_map_array would look like this: + * + * <- cpus cluster0 -><- cpus cluster1 -> + * --------------------------------------------------- + * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 | + * --------------------------------------------------- + * ^ ^ + * cluster __| cpu __| + * limit limit + * + * The first 2 entries are of the cluster nodes. The next 4 entries are of cpus + * within cluster 0. The last 4 entries are of cpus within cluster 1. + * The 'psci_aff_limits' array contains the max & min index of each affinity + * level within the 'psci_aff_map' array. This allows restricting search of a + * node at an affinity level between the indices in the limits array. + ******************************************************************************/ +void psci_setup(unsigned long mpidr) +{ + int afflvl, affmap_idx, rc, max_afflvl; + aff_map_node *node; + + /* Initialize psci's internal state */ + memset(psci_aff_map, 0, sizeof(psci_aff_map)); + memset(psci_aff_limits, 0, sizeof(psci_aff_limits)); + memset(psci_ns_entry_info, 0, sizeof(psci_ns_entry_info)); + psci_ns_einfo_idx = 0; + psci_plat_pm_ops = NULL; + + /* Find out the maximum affinity level that the platform implements */ + max_afflvl = get_max_afflvl(); + assert(max_afflvl <= MPIDR_MAX_AFFLVL); + + /* + * This call traverses the topology tree with help from the platform and + * populates the affinity map using a breadth-first-search recursively. + * We assume that the platform allocates affinity instance ids from 0 + * onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0 + */ + affmap_idx = 0; + for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) { + affmap_idx = psci_init_aff_map(FIRST_MPIDR, + affmap_idx, + max_afflvl, + afflvl); + } + + /* + * Set the bounds for the affinity counts of each level in the map. Also + * flush out the entire array so that it's visible to subsequent power + * management operations. The 'psci_aff_map' array is allocated in + * coherent memory so does not need flushing. The 'psci_aff_limits' + * array is allocated in normal memory. It will be accessed when the mmu + * is off e.g. after reset. Hence it needs to be flushed. + */ + for (afflvl = MPIDR_AFFLVL0; afflvl < max_afflvl; afflvl++) { + psci_aff_limits[afflvl].min = + psci_aff_limits[afflvl + 1].max + 1; + } + + flush_dcache_range((unsigned long) psci_aff_limits, + sizeof(psci_aff_limits)); + + /* + * Mark the affinity instances in our mpidr as ON. No need to lock as + * this is the primary cpu. + */ + mpidr &= MPIDR_AFFINITY_MASK; + for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) { + + node = psci_get_aff_map_node(mpidr, afflvl); + assert(node); + + /* Mark each present node as ON. */ + if (node->state & PSCI_AFF_PRESENT) { + psci_set_state(node->state, PSCI_STATE_ON); + } + } + + rc = platform_setup_pm(&psci_plat_pm_ops); + assert(rc == 0); + assert(psci_plat_pm_ops); + + return; +} |