diff options
| -rw-r--r-- | docs/psci-pd-tree.md | 295 | ||||
| -rw-r--r-- | include/bl31/services/psci1.0/psci.h | 31 | ||||
| -rw-r--r-- | include/plat/common/psci1.0/platform.h | 3 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_common.c | 314 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_helpers.S | 3 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_main.c | 34 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_off.c | 41 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_on.c | 76 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_private.h | 151 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_setup.c | 412 | ||||
| -rw-r--r-- | services/std_svc/psci1.0/psci_suspend.c | 59 |
11 files changed, 758 insertions, 661 deletions
diff --git a/docs/psci-pd-tree.md b/docs/psci-pd-tree.md new file mode 100644 index 00000000..6ae686d8 --- /dev/null +++ b/docs/psci-pd-tree.md @@ -0,0 +1,295 @@ +------------ +Requirements +------------ + +1. A platform must export the `plat_get_aff_count()` and + `plat_get_aff_state()` APIs to enable the generic PSCI code to + populate a tree that describes the hierarchy of power domains in the + system. This approach is inflexible because a change to the topology + requires a change in the code. + + It would be much simpler for the platform to describe its power domain tree + in a data structure. + +2. The generic PSCI code generates MPIDRs in order to populate the power domain + tree. It also uses an MPIDR to find a node in the tree. The assumption that + a platform will use exactly the same MPIDRs as generated by the generic PSCI + code is not scalable. The use of an MPIDR also restricts the number of + levels in the power domain tree to four. + + Therefore, there is a need to decouple allocation of MPIDRs from the + mechanism used to populate the power domain topology tree. + +3. The current arrangement of the power domain tree requires a binary search + over the sibling nodes at a particular level to find a specified power + domain node. During a power management operation, the tree is traversed from + a 'start' to an 'end' power level. The binary search is required to find the + node at each level. The natural way to perform this traversal is to + start from a leaf node and follow the parent node pointer to reach the end + level. + + Therefore, there is a need to define data structures that implement the tree in + a way which facilitates such a traversal. + +4. The attributes of a core power domain differ from the attributes of power + domains at higher levels. For example, only a core power domain can be identified + using an MPIDR. There is no requirement to perform state coordination while + performing a power management operation on the core power domain. + + Therefore, there is a need to implement the tree in a way which facilitates this + distinction between a leaf and non-leaf node and any associated + optimizations. + + +------ +Design +------ + +### Describing a power domain tree + +To fulfill requirement 1., the existing platform APIs +`plat_get_aff_count()` and `plat_get_aff_state()` have been +removed. A platform must define an array of unsigned chars such that: + +1. The first entry in the array specifies the number of power domains at the + highest power level implemented in the platform. This caters for platforms + where the power domain tree does not have a single root node, for example, + the FVP has two cluster power domains at the highest level (1). + +2. Each subsequent entry corresponds to a power domain and contains the number + of power domains that are its direct children. + +3. The size of the array minus the first entry will be equal to the number of + non-leaf power domains. + +4. The value in each entry in the array is used to find the number of entries + to consider at the next level. The sum of the values (number of children) of + all the entries at a level specifies the number of entries in the array for + the next level. + +The following example power domain topology tree will be used to describe the +above text further. The leaf and non-leaf nodes in this tree have been numbered +separately. + +``` + +-+ + |0| + +-+ + / \ + / \ + / \ + / \ + / \ + / \ + / \ + / \ + / \ + / \ + +-+ +-+ + |1| |2| + +-+ +-+ + / \ / \ + / \ / \ + / \ / \ + / \ / \ + +-+ +-+ +-+ +-+ + |3| |4| |5| |6| + +-+ +-+ +-+ +-+ + +---+-----+ +----+----| +----+----+ +----+-----+-----+ + | | | | | | | | | | | | | + | | | | | | | | | | | | | + v v v v v v v v v v v v v + +-+ +-+ +-+ +-+ +-+ +-+ +-+ +-+ +-+ +-+ +--+ +--+ +--+ + |0| |1| |2| |3| |4| |5| |6| |7| |8| |9| |10| |11| |12| + +-+ +-+ +-+ +-+ +-+ +-+ +-+ +-+ +-+ +-+ +--+ +--+ +--+ +``` + + +This tree is defined by the platform as the array described above as follows: + +``` + #define PLAT_NUM_POWER_DOMAINS 20 + #define PLATFORM_CORE_COUNT 13 + #define PSCI_NUM_NON_CPU_PWR_DOMAINS \ + (PLAT_NUM_POWER_DOMAINS - PLATFORM_CORE_COUNT) + + unsigned char plat_power_domain_tree_desc[] = { 1, 2, 2, 2, 3, 3, 3, 4}; +``` + +### Removing assumptions about MPIDRs used in a platform + +To fulfill requirement 2., it is assumed that the platform assigns a +unique number (core index) between `0` and `PLAT_CORE_COUNT - 1` to each core +power domain. MPIDRs could be allocated in any manner and will not be used to +populate the tree. + +`plat_core_pos_by_mpidr(mpidr)` will return the core index for the core +corresponding to the MPIDR. It will return an error (-1) if an MPIDR is passed +which is not allocated or corresponds to an absent core. The semantics of this +platform API have changed since it is required to validate the passed MPIDR. It +has been made a mandatory API as a result. + +Another mandatory API, `plat_my_core_pos()` has been added to return the core +index for the calling core. This API provides a more lightweight mechanism to get +the index since there is no need to validate the MPIDR of the calling core. + +The platform should assign the core indices (as illustrated in the diagram above) +such that, if the core nodes are numbered from left to right, then the index +for a core domain will be the same as the index returned by + `plat_core_pos_by_mpidr()` or `plat_my_core_pos()` for that core. This +relationship allows the core nodes to be allocated in a separate array +(requirement 4.) during `psci_setup()` in such an order that the index of the +core in the array is the same as the return value from these APIs. + +#### Dealing with holes in MPIDR allocation + +For platforms where the number of allocated MPIDRs is equal to the number of +core power domains, for example, Juno and FVPs, the logic to convert an MPIDR to +a core index should remain unchanged. Both Juno and FVP use a simple collision +proof hash function to do this. + +It is possible that on some platforms, the allocation of MPIDRs is not +contiguous or certain cores have been disabled. This essentially means that the +MPIDRs have been sparsely allocated, that is, the size of the range of MPIDRs +used by the platform is not equal to the number of core power domains. + +The platform could adopt one of the following approaches to deal with this +scenario: + +1. Implement more complex logic to convert a valid MPIDR to a core index while + maintaining the relationship described earlier. This means that the power + domain tree descriptor will not describe any core power domains which are + disabled or absent. Entries will not be allocated in the tree for these + domains. + +2. Treat unallocated MPIDRs and disabled cores as absent but still describe them + in the power domain descriptor, that is, the number of core nodes described + is equal to the size of the range of MPIDRs allocated. This approach will + lead to memory wastage since entries will be allocated in the tree but will + allow use of a simpler logic to convert an MPIDR to a core index. + + +### Traversing through and distinguishing between core and non-core power domains + +To fulfill requirement 3 and 4, separate data structures have been defined +to represent leaf and non-leaf power domain nodes in the tree. + +``` +/******************************************************************************* + * The following two data structures implement the power domain tree. The tree + * is used to track the state of all the nodes i.e. power domain instances + * described by the platform. The tree consists of nodes that describe CPU power + * domains i.e. leaf nodes and all other power domains which are parents of a + * CPU power domain i.e. non-leaf nodes. + ******************************************************************************/ +typedef struct non_cpu_pwr_domain_node { + /* + * Index of the first CPU power domain node level 0 which has this node + * as its parent. + */ + unsigned int cpu_start_idx; + + /* + * Number of CPU power domains which are siblings of the domain indexed + * by 'cpu_start_idx' i.e. all the domains in the range 'cpu_start_idx + * -> cpu_start_idx + ncpus' have this node as their parent. + */ + unsigned int ncpus; + + /* Index of the parent power domain node */ + unsigned int parent_node; + + ----- +} non_cpu_pd_node_t; + +typedef struct cpu_pwr_domain_node { + unsigned long mpidr; + + /* Index of the parent power domain node */ + unsigned int parent_node; + + ----- +} cpu_pd_node_t; +``` + +The power domain tree is implemented as a combination of the following data +structures. + +``` +non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS]; +cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT]; +``` + +### Populating the power domain tree + +The `populate_power_domain_tree()` function in `psci_setup.c` implements the +algorithm to parse the power domain descriptor exported by the platform to +populate the two arrays. It is essentially a breadth-first-search. The nodes for +each level starting from the root are laid out one after another in the +`psci_non_cpu_pd_nodes` and `psci_cpu_pd_nodes` arrays as follows: + +``` +psci_non_cpu_pd_nodes -> [[Level 3 nodes][Level 2 nodes][Level 1 nodes]] +psci_cpu_pd_nodes -> [Level 0 nodes] +``` + +For the example power domain tree illustrated above, the `psci_cpu_pd_nodes` +will be populated as follows. The value in each entry is the index of the parent +node. Other fields have been ignored for simplicity. + +``` + +-------------+ ^ + CPU0 | 3 | | + +-------------+ | + CPU1 | 3 | | + +-------------+ | + CPU2 | 3 | | + +-------------+ | + CPU3 | 4 | | + +-------------+ | + CPU4 | 4 | | + +-------------+ | + CPU5 | 4 | | PLATFORM_CORE_COUNT + +-------------+ | + CPU6 | 5 | | + +-------------+ | + CPU7 | 5 | | + +-------------+ | + CPU8 | 5 | | + +-------------+ | + CPU9 | 6 | | + +-------------+ | + CPU10 | 6 | | + +-------------+ | + CPU11 | 6 | | + +-------------+ | + CPU12 | 6 | v + +-------------+ +``` + +The `psci_non_cpu_pd_nodes` array will be populated as follows. The value in +each entry is the index of the parent node. + +``` + +-------------+ ^ + PD0 | -1 | | + +-------------+ | + PD1 | 0 | | + +-------------+ | + PD2 | 0 | | + +-------------+ | + PD3 | 1 | | PLAT_NUM_POWER_DOMAINS - + +-------------+ | PLATFORM_CORE_COUNT + PD4 | 1 | | + +-------------+ | + PD5 | 2 | | + +-------------+ | + PD6 | 2 | | + +-------------+ v +``` + +Each core can find its node in the `psci_cpu_pd_nodes` array using the +`plat_my_core_pos()` function. When a core is turned on, the normal world +provides an MPIDR. The `plat_core_pos_by_mpidr()` function is used to validate +the MPIDR before using it to find the corresponding core node. The non-core power +domain nodes do not need to be identified. diff --git a/include/bl31/services/psci1.0/psci.h b/include/bl31/services/psci1.0/psci.h index d00796c5..c31562c0 100644 --- a/include/bl31/services/psci1.0/psci.h +++ b/include/bl31/services/psci1.0/psci.h @@ -43,6 +43,19 @@ #define PSCI_NUM_PWR_DOMAINS (2 * PLATFORM_CORE_COUNT) #endif +#define PSCI_NUM_NON_CPU_PWR_DOMAINS (PSCI_NUM_PWR_DOMAINS - \ + PLATFORM_CORE_COUNT) + +/* This is the power level corresponding to a CPU */ +#define PSCI_CPU_PWR_LVL 0 + +/* + * The maximum power level supported by PSCI. Since PSCI CPU_SUSPEND + * uses the old power_state parameter format which has 2 bits to specify the + * power level, this constant is defined to be 3. + */ +#define PSCI_MAX_PWR_LVL 3 + /******************************************************************************* * Defines for runtime services func ids ******************************************************************************/ @@ -137,16 +150,11 @@ #define PSCI_E_NOT_PRESENT -7 #define PSCI_E_DISABLED -8 +#define PSCI_INVALID_MPIDR ~(0ULL) + /******************************************************************************* - * PSCI power domain state related constants. A power domain instance could - * be present or absent physically to cater for asymmetric topologies. If - * present then it could be in one of the 4 further defined states. + * PSCI power domain state related constants. ******************************************************************************/ -#define PSCI_STATE_SHIFT 1 -#define PSCI_STATE_MASK 0xff - -#define PSCI_PWR_DOMAIN_ABSENT 0x0 -#define PSCI_PWR_DOMAIN_PRESENT 0x1 #define PSCI_STATE_ON 0x0 #define PSCI_STATE_OFF 0x1 #define PSCI_STATE_ON_PENDING 0x2 @@ -170,9 +178,10 @@ * this information will not reside on a cache line shared with another cpu. ******************************************************************************/ typedef struct psci_cpu_data { - uint32_t power_state; + uint32_t power_state; /* The power state from CPU_SUSPEND */ + unsigned char psci_state; /* The state of this CPU as seen by PSCI */ #if !USE_COHERENT_MEM - bakery_info_t pcpu_bakery_info[PSCI_NUM_PWR_DOMAINS]; + bakery_info_t pcpu_bakery_info[PSCI_NUM_NON_CPU_PWR_DOMAINS]; #endif } psci_cpu_data_t; @@ -230,7 +239,7 @@ void __dead2 psci_power_down_wfi(void); void psci_cpu_on_finish_entry(void); void psci_cpu_suspend_finish_entry(void); void psci_register_spd_pm_hook(const spd_pm_ops_t *); -int psci_get_suspend_stateid_by_mpidr(unsigned long); +int psci_get_suspend_stateid_by_idx(unsigned long); int psci_get_suspend_stateid(void); int psci_get_suspend_pwrlvl(void); diff --git a/include/plat/common/psci1.0/platform.h b/include/plat/common/psci1.0/platform.h index 059320f1..acc5b952 100644 --- a/include/plat/common/psci1.0/platform.h +++ b/include/plat/common/psci1.0/platform.h @@ -183,8 +183,7 @@ struct entry_point_info *bl31_plat_get_next_image_ep_info(uint32_t type); * Mandatory PSCI functions (BL3-1) ******************************************************************************/ int platform_setup_pm(const struct plat_pm_ops **); -unsigned int plat_get_pwr_domain_count(unsigned int, unsigned long); -unsigned int plat_get_pwr_domain_state(unsigned int, unsigned long); +const unsigned char *plat_get_power_domain_tree_desc(void); /******************************************************************************* * Optional BL3-1 functions (may be overridden) diff --git a/services/std_svc/psci1.0/psci_common.c b/services/std_svc/psci1.0/psci_common.c index d9e9ccee..4812bc46 100644 --- a/services/std_svc/psci1.0/psci_common.c +++ b/services/std_svc/psci1.0/psci_common.c @@ -46,16 +46,20 @@ const spd_pm_ops_t *psci_spd_pm; /******************************************************************************* - * Grand array that holds the platform's topology information for state - * management of power domain instances. Each node (pwr_map_node) in the array - * corresponds to a power domain instance e.g. cluster, cpu within an mpidr + * Arrays that hold the platform's power domain tree information for state + * management of power domains. + * Each node in the array 'psci_non_cpu_pd_nodes' corresponds to a power domain + * which is an ancestor of a CPU power domain. + * Each node in the array 'psci_cpu_pd_nodes' corresponds to a cpu power domain ******************************************************************************/ -pwr_map_node_t psci_pwr_domain_map[PSCI_NUM_PWR_DOMAINS] +non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS] #if USE_COHERENT_MEM __attribute__ ((section("tzfw_coherent_mem"))) #endif ; +cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT]; + /******************************************************************************* * Pointer to functions exported by the platform to complete power mgmt. ops ******************************************************************************/ @@ -64,29 +68,31 @@ const plat_pm_ops_t *psci_plat_pm_ops; /******************************************************************************* * Check that the maximum power level supported by the platform makes sense * ****************************************************************************/ -CASSERT(PLAT_MAX_PWR_LVL <= MPIDR_MAX_AFFLVL && \ - PLAT_MAX_PWR_LVL >= MPIDR_AFFLVL0, \ +CASSERT(PLAT_MAX_PWR_LVL <= PSCI_MAX_PWR_LVL && \ + PLAT_MAX_PWR_LVL >= PSCI_CPU_PWR_LVL, \ assert_platform_max_pwrlvl_check); /******************************************************************************* - * This function is passed an array of pointers to power domain nodes in the - * topology tree for an mpidr. It iterates through the nodes to find the - * highest power level where the power domain is marked as physically powered - * off. + * This function is passed a cpu_index and the highest level in the topology + * tree. It iterates through the nodes to find the highest power level at which + * a domain is physically powered off. ******************************************************************************/ -uint32_t psci_find_max_phys_off_pwrlvl(uint32_t start_pwrlvl, - uint32_t end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]) +uint32_t psci_find_max_phys_off_pwrlvl(uint32_t end_pwrlvl, + unsigned int cpu_idx) { - uint32_t max_pwrlvl = PSCI_INVALID_DATA; + int max_pwrlvl, level; + unsigned int parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node; - for (; start_pwrlvl <= end_pwrlvl; start_pwrlvl++) { - if (mpidr_nodes[start_pwrlvl] == NULL) - continue; + if (psci_get_phys_state(cpu_idx, PSCI_CPU_PWR_LVL) != PSCI_STATE_OFF) + return PSCI_INVALID_DATA; + + max_pwrlvl = PSCI_CPU_PWR_LVL; + + for (level = PSCI_CPU_PWR_LVL + 1; level <= end_pwrlvl; level++) { + if (psci_get_phys_state(parent_idx, level) == PSCI_STATE_OFF) + max_pwrlvl = level; - if (psci_get_phys_state(mpidr_nodes[start_pwrlvl]) == - PSCI_STATE_OFF) - max_pwrlvl = start_pwrlvl; + parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node; } return max_pwrlvl; @@ -103,21 +109,14 @@ unsigned int psci_is_last_on_cpu(void) unsigned long mpidr = read_mpidr_el1() & MPIDR_AFFINITY_MASK; unsigned int i; - for (i = psci_pwr_lvl_limits[MPIDR_AFFLVL0].min; - i <= psci_pwr_lvl_limits[MPIDR_AFFLVL0].max; i++) { - - assert(psci_pwr_domain_map[i].level == MPIDR_AFFLVL0); - - if (!(psci_pwr_domain_map[i].state & PSCI_AFF_PRESENT)) - continue; - - if (psci_pwr_domain_map[i].mpidr == mpidr) { - assert(psci_get_state(&psci_pwr_domain_map[i]) + for (i = 0; i < PLATFORM_CORE_COUNT; i++) { + if (psci_cpu_pd_nodes[i].mpidr == mpidr) { + assert(psci_get_state(i, PSCI_CPU_PWR_LVL) == PSCI_STATE_ON); continue; } - if (psci_get_state(&psci_pwr_domain_map[i]) != PSCI_STATE_OFF) + if (psci_get_state(i, PSCI_CPU_PWR_LVL) != PSCI_STATE_OFF) return 0; } @@ -135,18 +134,12 @@ int get_power_on_target_pwrlvl(void) #if DEBUG unsigned int state; - pwr_map_node_t *node; - - /* Retrieve our node from the topology tree */ - node = psci_get_pwr_map_node(read_mpidr_el1() & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0); - assert(node); /* * Sanity check the state of the cpu. It should be either suspend or "on * pending" */ - state = psci_get_state(node); + state = psci_get_state(plat_my_core_pos(), PSCI_CPU_PWR_LVL); assert(state == PSCI_STATE_SUSPEND || state == PSCI_STATE_ON_PENDING); #endif @@ -163,103 +156,74 @@ int get_power_on_target_pwrlvl(void) } /******************************************************************************* - * Simple routine to set the id of a power domain instance at a given level - * in the mpidr. The assumption is that the affinity level and the power - * level are the same. - ******************************************************************************/ -unsigned long mpidr_set_pwr_domain_inst(unsigned long mpidr, - unsigned char pwr_inst, - int pwr_lvl) -{ - unsigned long aff_shift; - - assert(pwr_lvl <= MPIDR_AFFLVL3); - - /* - * Decide the number of bits to shift by depending upon - * the power level - */ - aff_shift = get_afflvl_shift(pwr_lvl); - - /* Clear the existing affinity instance & set the new one*/ - mpidr &= ~(((unsigned long)MPIDR_AFFLVL_MASK) << aff_shift); - mpidr |= ((unsigned long)pwr_inst) << aff_shift; - - return mpidr; -} - -/******************************************************************************* - * This function sanity checks a range of power levels. + * PSCI helper function to get the parent nodes corresponding to a cpu_index. ******************************************************************************/ -int psci_check_pwrlvl_range(int start_pwrlvl, int end_pwrlvl) +void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx, + int end_lvl, + unsigned int node_index[]) { - /* Sanity check the parameters passed */ - if (end_pwrlvl > PLAT_MAX_PWR_LVL) - return PSCI_E_INVALID_PARAMS; + unsigned int parent_node = psci_cpu_pd_nodes[cpu_idx].parent_node; + int i; - if (start_pwrlvl < MPIDR_AFFLVL0) - return PSCI_E_INVALID_PARAMS; - - if (end_pwrlvl < start_pwrlvl) - return PSCI_E_INVALID_PARAMS; - - return PSCI_E_SUCCESS; + for (i = PSCI_CPU_PWR_LVL + 1; i <= end_lvl; i++) { + *node_index++ = parent_node; + parent_node = psci_non_cpu_pd_nodes[parent_node].parent_node; + } } /******************************************************************************* - * This function is passed an array of pointers to power domain nodes in the - * topology tree for an mpidr and the state which each node should transition - * to. It updates the state of each node between the specified power levels. + * This function is passed a cpu_index and the highest level in the topology + * tree and the state which each node should transition to. It updates the + * state of each node between the specified power levels. ******************************************************************************/ -void psci_do_state_coordination(uint32_t start_pwrlvl, - uint32_t end_pwrlvl, - pwr_map_node_t *mpidr_nodes[], - uint32_t state) +void psci_do_state_coordination(int end_pwrlvl, + unsigned int cpu_idx, + uint32_t state) { - uint32_t level; + int level; + unsigned int parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node; + psci_set_state(cpu_idx, state, PSCI_CPU_PWR_LVL); - for (level = start_pwrlvl; level <= end_pwrlvl; level++) { - if (mpidr_nodes[level] == NULL) - continue; - psci_set_state(mpidr_nodes[level], state); + for (level = PSCI_CPU_PWR_LVL + 1; level <= end_pwrlvl; level++) { + psci_set_state(parent_idx, state, level); + parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node; } } /******************************************************************************* - * This function is passed an array of pointers to power domain nodes in the - * topology tree for an mpidr. It picks up locks for each power level bottom - * up in the range specified. + * This function is passed a cpu_index and the highest level in the topology + * tree that the operation should be applied to. It picks up locks in order of + * increasing power domain level in the range specified. ******************************************************************************/ -void psci_acquire_pwr_domain_locks(int start_pwrlvl, - int end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]) +void psci_acquire_pwr_domain_locks(int end_pwrlvl, unsigned int cpu_idx) { + unsigned int parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node; int level; - for (level = start_pwrlvl; level <= end_pwrlvl; level++) { - if (mpidr_nodes[level] == NULL) - continue; - - psci_lock_get(mpidr_nodes[level]); + /* No locking required for level 0. Hence start locking from level 1 */ + for (level = PSCI_CPU_PWR_LVL + 1; level <= end_pwrlvl; level++) { + psci_lock_get(&psci_non_cpu_pd_nodes[parent_idx]); + parent_idx = psci_non_cpu_pd_nodes[parent_idx].parent_node; } } /******************************************************************************* - * This function is passed an array of pointers to power domain nodes in the - * topology tree for an mpidr. It releases the lock for each power level top - * down in the range specified. + * This function is passed a cpu_index and the highest level in the topology + * tree that the operation should be applied to. It releases the locks in order + * of decreasing power domain level in the range specified. ******************************************************************************/ -void psci_release_pwr_domain_locks(int start_pwrlvl, - int end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]) +void psci_release_pwr_domain_locks(int end_pwrlvl, unsigned int cpu_idx) { + unsigned int parent_idx, parent_nodes[PLAT_MAX_PWR_LVL] = {0}; int level; - for (level = end_pwrlvl; level >= start_pwrlvl; level--) { - if (mpidr_nodes[level] == NULL) - continue; + /* Get the parent nodes */ + psci_get_parent_pwr_domain_nodes(cpu_idx, end_pwrlvl, parent_nodes); - psci_lock_release(mpidr_nodes[level]); + /* Unlock top down. No unlocking required for level 0. */ + for (level = end_pwrlvl; level >= PSCI_CPU_PWR_LVL + 1; level--) { + parent_idx = parent_nodes[level - 1]; + psci_lock_release(&psci_non_cpu_pd_nodes[parent_idx]); } } @@ -332,21 +296,22 @@ int psci_get_ns_ep_info(entry_point_info_t *ep, } /******************************************************************************* - * This function takes a pointer to a power domain node in the topology tree - * and returns its state. State of a non-leaf node needs to be calculated. + * This function takes an index and level of a power domain node in the topology + * tree and returns its state. State of a non-leaf node needs to be calculated. ******************************************************************************/ -unsigned short psci_get_state(pwr_map_node_t *node) +unsigned short psci_get_state(unsigned int idx, + int level) { -#if !USE_COHERENT_MEM - flush_dcache_range((uint64_t) node, sizeof(*node)); -#endif - - assert(node->level >= MPIDR_AFFLVL0 && node->level <= MPIDR_MAX_AFFLVL); - /* A cpu node just contains the state which can be directly returned */ - if (node->level == MPIDR_AFFLVL0) - return (node->state >> PSCI_STATE_SHIFT) & PSCI_STATE_MASK; + if (level == PSCI_CPU_PWR_LVL) { + flush_cpu_data_by_index(idx, psci_svc_cpu_data.psci_state); + return get_cpu_data_by_index(idx, psci_svc_cpu_data.psci_state); + } +#if !USE_COHERENT_MEM + flush_dcache_range((uint64_t) &psci_non_cpu_pd_nodes[idx], + sizeof(psci_non_cpu_pd_nodes[idx])); +#endif /* * For a power level higher than a cpu, the state has to be * calculated. It depends upon the value of the reference count @@ -355,35 +320,35 @@ unsigned short psci_get_state(pwr_map_node_t *node) * count. If the reference count is 0 then the power level is * OFF else ON. */ - if (node->ref_count) + if (psci_non_cpu_pd_nodes[idx].ref_count) return PSCI_STATE_ON; else return PSCI_STATE_OFF; } /******************************************************************************* - * This function takes a pointer to a power domain node in the topology - * tree and a target state. State of a non-leaf node needs to be converted - * to a reference count. State of a leaf node can be set directly. + * This function takes an index and level of a power domain node in the topology + * tree and a target state. State of a non-leaf node needs to be converted to + * a reference count. State of a leaf node can be set directly. ******************************************************************************/ -void psci_set_state(pwr_map_node_t *node, unsigned short state) +void psci_set_state(unsigned int idx, + unsigned short state, + int level) { - assert(node->level >= MPIDR_AFFLVL0 && node->level <= MPIDR_MAX_AFFLVL); - /* * For a power level higher than a cpu, the state is used * to decide whether the reference count is incremented or * decremented. Entry into the ON_PENDING state does not have * effect. */ - if (node->level > MPIDR_AFFLVL0) { + if (level > PSCI_CPU_PWR_LVL) { switch (state) { case PSCI_STATE_ON: - node->ref_count++; + psci_non_cpu_pd_nodes[idx].ref_count++; break; case PSCI_STATE_OFF: case PSCI_STATE_SUSPEND: - node->ref_count--; + psci_non_cpu_pd_nodes[idx].ref_count--; break; case PSCI_STATE_ON_PENDING: /* @@ -393,15 +358,16 @@ void psci_set_state(pwr_map_node_t *node, unsigned short state) return; default: assert(0); - } - } else { - node->state &= ~(PSCI_STATE_MASK << PSCI_STATE_SHIFT); - node->state |= (state & PSCI_STATE_MASK) << PSCI_STATE_SHIFT; - } #if !USE_COHERENT_MEM - flush_dcache_range((uint64_t) node, sizeof(*node)); + flush_dcache_range((uint64_t) &psci_non_cpu_pd_nodes[idx], + sizeof(psci_non_cpu_pd_nodes[idx])); #endif + } + } else { + set_cpu_data_by_index(idx, psci_svc_cpu_data.psci_state, state); + flush_cpu_data_by_index(idx, psci_svc_cpu_data.psci_state); + } } /******************************************************************************* @@ -411,11 +377,12 @@ void psci_set_state(pwr_map_node_t *node, unsigned short state) * tell whether that's actually happened or not. So we err on the side of * caution & treat the power domain as being turned off. ******************************************************************************/ -unsigned short psci_get_phys_state(pwr_map_node_t *node) +unsigned short psci_get_phys_state(unsigned int idx, + int level) { unsigned int state; - state = psci_get_state(node); + state = psci_get_state(idx, level); return get_phys_state(state); } @@ -429,60 +396,41 @@ unsigned short psci_get_phys_state(pwr_map_node_t *node) * coherency at the interconnect level in addition to gic cpu interface. ******************************************************************************/ void psci_power_up_finish(int end_pwrlvl, - pwrlvl_power_on_finisher_t pon_handler) + pwrlvl_power_on_finisher_t pon_handler) { - mpidr_pwr_map_nodes_t mpidr_nodes; - int rc; + unsigned int cpu_idx = plat_my_core_pos(); unsigned int max_phys_off_pwrlvl; - - /* - * Collect the pointers to the nodes in the topology tree for - * each power domain instances in the mpidr. If this function does - * not return successfully then either the mpidr or the power - * levels are incorrect. Either case is an irrecoverable error. - */ - rc = psci_get_pwr_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); - if (rc != PSCI_E_SUCCESS) - panic(); - /* * This function acquires the lock corresponding to each power * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + psci_acquire_pwr_domain_locks(end_pwrlvl, + cpu_idx); - max_phys_off_pwrlvl = psci_find_max_phys_off_pwrlvl(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + max_phys_off_pwrlvl = psci_find_max_phys_off_pwrlvl(end_pwrlvl, + cpu_idx); assert(max_phys_off_pwrlvl != PSCI_INVALID_DATA); /* Perform generic, architecture and platform specific handling */ - pon_handler(mpidr_nodes, max_phys_off_pwrlvl); + pon_handler(cpu_idx, max_phys_off_pwrlvl); /* * This function updates the state of each power instance - * corresponding to the mpidr in the range of power levels + * corresponding to the cpu index in the range of power levels * specified. */ - psci_do_state_coordination(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes, - PSCI_STATE_ON); + psci_do_state_coordination(end_pwrlvl, + cpu_idx, + PSCI_STATE_ON); /* * This loop releases the lock corresponding to each power level * in the reverse order to which they were acquired. */ - psci_release_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + psci_release_pwr_domain_locks(end_pwrlvl, + cpu_idx); } /******************************************************************************* @@ -533,8 +481,8 @@ int psci_spd_migrate_info(uint64_t *mpidr) void psci_print_power_domain_map(void) { #if LOG_LEVEL >= LOG_LEVEL_INFO - pwr_map_node_t *node; - unsigned int idx; + unsigned int idx, state; + /* This array maps to the PSCI_STATE_X definitions in psci.h */ static const char *psci_state_str[] = { "ON", @@ -544,14 +492,20 @@ void psci_print_power_domain_map(void) }; INFO("PSCI Power Domain Map:\n"); - for (idx = 0; idx < PSCI_NUM_PWR_DOMAINS; idx++) { - node = &psci_pwr_domain_map[idx]; - if (!(node->state & PSCI_PWR_DOMAIN_PRESENT)) { - continue; - } - INFO(" pwrInst: Level %u, MPID 0x%lx, State %s\n", - node->level, node->mpidr, - psci_state_str[psci_get_state(node)]); + for (idx = 0; idx < (PSCI_NUM_PWR_DOMAINS - PLATFORM_CORE_COUNT); idx++) { + state = psci_get_state(idx, psci_non_cpu_pd_nodes[idx].level); + INFO(" Domain Node : Level %u, parent_node %d, State %s\n", + psci_non_cpu_pd_nodes[idx].level, + psci_non_cpu_pd_nodes[idx].parent_node, + psci_state_str[state]); + } + + for (idx = 0; idx < PLATFORM_CORE_COUNT; idx++) { + state = psci_get_state(idx, PSCI_CPU_PWR_LVL); + INFO(" CPU Node : MPID 0x%lx, parent_node %d, State %s\n", + psci_cpu_pd_nodes[idx].mpidr, + psci_cpu_pd_nodes[idx].parent_node, + psci_state_str[state]); } #endif } diff --git a/services/std_svc/psci1.0/psci_helpers.S b/services/std_svc/psci1.0/psci_helpers.S index 7ef4cdd6..bbfa5d5d 100644 --- a/services/std_svc/psci1.0/psci_helpers.S +++ b/services/std_svc/psci1.0/psci_helpers.S @@ -28,7 +28,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include <arch.h> #include <asm_macros.S> #include <assert_macros.S> #include <platform_def.h> @@ -67,7 +66,7 @@ func psci_do_pwrdown_cache_maintenance * platform. * --------------------------------------------- */ - cmp x0, #MPIDR_AFFLVL0 + cmp x0, #PSCI_CPU_PWR_LVL b.eq do_core_pwr_dwn bl prepare_cluster_pwr_dwn b do_stack_maintenance diff --git a/services/std_svc/psci1.0/psci_main.c b/services/std_svc/psci1.0/psci_main.c index f87fd52e..5732da20 100644 --- a/services/std_svc/psci1.0/psci_main.c +++ b/services/std_svc/psci1.0/psci_main.c @@ -240,32 +240,26 @@ int psci_cpu_off(void) int psci_affinity_info(unsigned long target_affinity, unsigned int lowest_affinity_level) { - int rc = PSCI_E_INVALID_PARAMS; - unsigned int pwr_domain_state; - pwr_map_node_t *node; + unsigned int cpu_idx; + unsigned char cpu_pwr_domain_state; - if (lowest_affinity_level > PLAT_MAX_PWR_LVL) - return rc; - - node = psci_get_pwr_map_node(target_affinity, lowest_affinity_level); - if (node && (node->state & PSCI_PWR_DOMAIN_PRESENT)) { + /* We dont support level higher than PSCI_CPU_PWR_LVL */ + if (lowest_affinity_level > PSCI_CPU_PWR_LVL) + return PSCI_E_INVALID_PARAMS; - /* - * TODO: For power levels higher than 0 i.e. cpu, the - * state will always be either ON or OFF. Need to investigate - * how critical is it to support ON_PENDING here. - */ - pwr_domain_state = psci_get_state(node); + /* Calculate the cpu index of the target */ + cpu_idx = plat_core_pos_by_mpidr(target_affinity); + if (cpu_idx == -1) + return PSCI_E_INVALID_PARAMS; - /* A suspended cpu is available & on for the OS */ - if (pwr_domain_state == PSCI_STATE_SUSPEND) { - pwr_domain_state = PSCI_STATE_ON; - } + cpu_pwr_domain_state = psci_get_state(cpu_idx, PSCI_CPU_PWR_LVL); - rc = pwr_domain_state; + /* A suspended cpu is available & on for the OS */ + if (cpu_pwr_domain_state == PSCI_STATE_SUSPEND) { + cpu_pwr_domain_state = PSCI_STATE_ON; } - return rc; + return cpu_pwr_domain_state; } int psci_migrate(unsigned long target_cpu) diff --git a/services/std_svc/psci1.0/psci_off.c b/services/std_svc/psci1.0/psci_off.c index f4fb9809..78410f1d 100644 --- a/services/std_svc/psci1.0/psci_off.c +++ b/services/std_svc/psci1.0/psci_off.c @@ -32,6 +32,7 @@ #include <arch_helpers.h> #include <assert.h> #include <debug.h> +#include <platform.h> #include <string.h> #include "psci_private.h" @@ -50,8 +51,7 @@ ******************************************************************************/ int psci_do_cpu_off(int end_pwrlvl) { - int rc; - mpidr_pwr_map_nodes_t mpidr_nodes; + int rc, idx = plat_my_core_pos(); unsigned int max_phys_off_pwrlvl; /* @@ -61,27 +61,12 @@ int psci_do_cpu_off(int end_pwrlvl) assert(psci_plat_pm_ops->pwr_domain_off); /* - * Collect the pointers to the nodes in the topology tree for - * each power domain instance in the mpidr. If this function does - * not return successfully then either the mpidr or the power - * levels are incorrect. Either way, this an internal TF error - * therefore assert. - */ - rc = psci_get_pwr_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); - assert(rc == PSCI_E_SUCCESS); - - /* * This function acquires the lock corresponding to each power * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); - + psci_acquire_pwr_domain_locks(end_pwrlvl, + idx); /* * Call the cpu off handler registered by the Secure Payload Dispatcher @@ -96,17 +81,14 @@ int psci_do_cpu_off(int end_pwrlvl) /* * This function updates the state of each power domain instance - * corresponding to the mpidr in the range of power levels + * corresponding to the cpu index in the range of power levels * specified. */ - psci_do_state_coordination(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes, - PSCI_STATE_OFF); + psci_do_state_coordination(end_pwrlvl, + idx, + PSCI_STATE_OFF); - max_phys_off_pwrlvl = psci_find_max_phys_off_pwrlvl(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + max_phys_off_pwrlvl = psci_find_max_phys_off_pwrlvl(end_pwrlvl, idx); assert(max_phys_off_pwrlvl != PSCI_INVALID_DATA); /* @@ -126,9 +108,8 @@ exit: * Release the locks corresponding to each power level in the * reverse order to which they were acquired. */ - psci_release_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + psci_release_pwr_domain_locks(end_pwrlvl, + idx); /* * Check if all actions needed to safely power down this cpu have diff --git a/services/std_svc/psci1.0/psci_on.c b/services/std_svc/psci1.0/psci_on.c index c78e119a..c85e3493 100644 --- a/services/std_svc/psci1.0/psci_on.c +++ b/services/std_svc/psci1.0/psci_on.c @@ -71,8 +71,8 @@ int psci_cpu_on_start(unsigned long target_cpu, int end_pwrlvl) { int rc; - mpidr_pwr_map_nodes_t target_cpu_nodes; unsigned long psci_entrypoint; + unsigned int target_idx = plat_core_pos_by_mpidr(target_cpu); /* * This function must only be called on platforms where the @@ -81,33 +81,14 @@ int psci_cpu_on_start(unsigned long target_cpu, assert(psci_plat_pm_ops->pwr_domain_on && psci_plat_pm_ops->pwr_domain_on_finish); - /* - * Collect the pointers to the nodes in the topology tree for - * each power domain instance in the mpidr. If this function does - * not return successfully then either the mpidr or the power - * levels are incorrect. - */ - rc = psci_get_pwr_map_nodes(target_cpu, - MPIDR_AFFLVL0, - end_pwrlvl, - target_cpu_nodes); - assert(rc == PSCI_E_SUCCESS); - - /* - * This function acquires the lock corresponding to each power - * level so that by the time all locks are taken, the system topology - * is snapshot and state management can be done safely. - */ - psci_acquire_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - target_cpu_nodes); + /* Protect against multiple CPUs trying to turn ON the same target CPU */ + psci_spin_lock_cpu(target_idx); /* * Generic management: Ensure that the cpu is off to be * turned on. */ - rc = cpu_on_validate_state(psci_get_state( - target_cpu_nodes[MPIDR_AFFLVL0])); + rc = cpu_on_validate_state(psci_get_state(target_idx, PSCI_CPU_PWR_LVL)); if (rc != PSCI_E_SUCCESS) goto exit; @@ -121,13 +102,12 @@ int psci_cpu_on_start(unsigned long target_cpu, /* * This function updates the state of each affinity instance - * corresponding to the mpidr in the range of affinity levels + * corresponding to the mpidr in the range of power domain levels * specified. */ - psci_do_state_coordination(MPIDR_AFFLVL0, - end_pwrlvl, - target_cpu_nodes, - PSCI_STATE_ON_PENDING); + psci_do_state_coordination(end_pwrlvl, + target_idx, + PSCI_STATE_ON_PENDING); /* * Perform generic, architecture and platform specific handling. @@ -150,20 +130,12 @@ int psci_cpu_on_start(unsigned long target_cpu, cm_init_context_by_index(target_idx, ep); else /* Restore the state on error. */ - psci_do_state_coordination(MPIDR_AFFLVL0, - end_pwrlvl, - target_cpu_nodes, - PSCI_STATE_OFF); + psci_do_state_coordination(end_pwrlvl, + target_idx, + PSCI_STATE_OFF); exit: - /* - * This loop releases the lock corresponding to each power level - * in the reverse order to which they were acquired. - */ - psci_release_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - target_cpu_nodes); - + psci_spin_unlock_cpu(target_idx); return rc; } @@ -171,12 +143,12 @@ exit: * The following function finish an earlier power on request. They * are called by the common finisher routine in psci_common.c. ******************************************************************************/ -void psci_cpu_on_finish(pwr_map_node_t *node[], int pwrlvl) +void psci_cpu_on_finish(unsigned int cpu_idx, + int max_off_pwrlvl) { - assert(node[pwrlvl]->level == pwrlvl); - /* Ensure we have been explicitly woken up by another cpu */ - assert(psci_get_state(node[MPIDR_AFFLVL0]) == PSCI_STATE_ON_PENDING); + assert(psci_get_state(cpu_idx, PSCI_CPU_PWR_LVL) + == PSCI_STATE_ON_PENDING); /* * Plat. management: Perform the platform specific actions @@ -184,7 +156,7 @@ void psci_cpu_on_finish(pwr_map_node_t *node[], int pwrlvl) * register. The actual state of this cpu has already been * changed. */ - psci_plat_pm_ops->pwr_domain_on_finish(pwrlvl); + psci_plat_pm_ops->pwr_domain_on_finish(max_off_pwrlvl); /* * Arch. management: Enable data cache and manage stack memory @@ -199,6 +171,15 @@ void psci_cpu_on_finish(pwr_map_node_t *node[], int pwrlvl) bl31_arch_setup(); /* + * Lock the CPU spin lock to make sure that the context initialization + * is done. Since the lock is only used in this function to create + * a synchronization point with cpu_on_start(), it can be released + * immediately. + */ + psci_spin_lock_cpu(cpu_idx); + psci_spin_unlock_cpu(cpu_idx); + + /* * Call the cpu on finish handler registered by the Secure Payload * Dispatcher to let it do any bookeeping. If the handler encounters an * error, it's expected to assert within @@ -206,6 +187,10 @@ void psci_cpu_on_finish(pwr_map_node_t *node[], int pwrlvl) if (psci_spd_pm && psci_spd_pm->svc_on_finish) psci_spd_pm->svc_on_finish(0); + /* Populate the mpidr field within the cpu node array */ + /* This needs to be done only once */ + psci_cpu_pd_nodes[cpu_idx].mpidr = read_mpidr() & MPIDR_AFFINITY_MASK; + /* * Generic management: Now we just need to retrieve the * information that we had stashed away during the cpu_on @@ -216,4 +201,3 @@ void psci_cpu_on_finish(pwr_map_node_t *node[], int pwrlvl) /* Clean caches before re-entering normal world */ dcsw_op_louis(DCCSW); } - diff --git a/services/std_svc/psci1.0/psci_private.h b/services/std_svc/psci1.0/psci_private.h index 79909a86..e7ad711f 100644 --- a/services/std_svc/psci1.0/psci_private.h +++ b/services/std_svc/psci1.0/psci_private.h @@ -34,25 +34,30 @@ #include <arch.h> #include <bakery_lock.h> #include <bl_common.h> +#include <cpu_data.h> #include <psci.h> +#include <spinlock.h> /* * The following helper macros abstract the interface to the Bakery * Lock API. */ #if USE_COHERENT_MEM -#define psci_lock_init(pwr_map, idx) bakery_lock_init(&(pwr_map)[(idx)].lock) -#define psci_lock_get(node) bakery_lock_get(&((node)->lock)) -#define psci_lock_release(node) bakery_lock_release(&((node)->lock)) +#define psci_lock_init(non_cpu_pd_node, idx) \ + bakery_lock_init(&(non_cpu_pd_node)[(idx)].lock) +#define psci_lock_get(non_cpu_pd_node) \ + bakery_lock_get(&((non_cpu_pd_node)->lock)) +#define psci_lock_release(non_cpu_pd_node) \ + bakery_lock_release(&((non_cpu_pd_node)->lock)) #else -#define psci_lock_init(pwr_map, idx) \ - ((pwr_map)[(idx)].pwr_domain_index = (idx)) -#define psci_lock_get(node) \ - bakery_lock_get((node)->pwr_domain_index,\ - CPU_DATA_PSCI_LOCK_OFFSET) -#define psci_lock_release(node) \ - bakery_lock_release((node)->pwr_domain_index,\ - CPU_DATA_PSCI_LOCK_OFFSET) +#define psci_lock_init(non_cpu_pd_node, idx) \ + ((non_cpu_pd_node)[(idx)].lock_index = (idx)) +#define psci_lock_get(non_cpu_pd_node) \ + bakery_lock_get((non_cpu_pd_node)->lock_index, \ + CPU_DATA_PSCI_LOCK_OFFSET) +#define psci_lock_release(non_cpu_pd_node) \ + bakery_lock_release((non_cpu_pd_node)->lock_index, \ + CPU_DATA_PSCI_LOCK_OFFSET) #endif /* @@ -75,39 +80,76 @@ define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64) | \ define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64)) +/* + * Helper macros for the CPU level spinlocks + */ +#define psci_spin_lock_cpu(idx) spin_lock(&psci_cpu_pd_nodes[idx].cpu_lock) +#define psci_spin_unlock_cpu(idx) spin_unlock(&psci_cpu_pd_nodes[idx].cpu_lock) /******************************************************************************* - * The following two data structures hold the topology tree which in turn tracks - * the state of the all the power domain instances supported by the platform. + * The following two data structures implement the power domain tree. The tree + * is used to track the state of all the nodes i.e. power domain instances + * described by the platform. The tree consists of nodes that describe CPU power + * domains i.e. leaf nodes and all other power domains which are parents of a + * CPU power domain i.e. non-leaf nodes. ******************************************************************************/ -typedef struct pwr_map_node { - unsigned long mpidr; +typedef struct non_cpu_pwr_domain_node { + /* + * Index of the first CPU power domain node level 0 which has this node + * as its parent. + */ + unsigned int cpu_start_idx; + + /* + * Number of CPU power domains which are siblings of the domain indexed + * by 'cpu_start_idx' i.e. all the domains in the range 'cpu_start_idx + * -> cpu_start_idx + ncpus' have this node as their parent. + */ + unsigned int ncpus; + + /* + * Index of the parent power domain node. + * TODO: Figure out whether to whether using pointer is more efficient. + */ + unsigned int parent_node; + unsigned char ref_count; - unsigned char state; unsigned char level; #if USE_COHERENT_MEM bakery_lock_t lock; #else /* For indexing the bakery_info array in per CPU data */ - unsigned char pwr_domain_index; + unsigned char lock_index; #endif -} pwr_map_node_t; +} non_cpu_pd_node_t; -typedef struct pwr_lvl_limits_node { - int min; - int max; -} pwr_lvl_limits_node_t; +typedef struct cpu_pwr_domain_node { + unsigned long mpidr; -typedef pwr_map_node_t (*mpidr_pwr_map_nodes_t[MPIDR_MAX_AFFLVL + 1]); -typedef void (*pwrlvl_power_on_finisher_t)(pwr_map_node_t *mpidr_nodes[], - int pwrlvl); + /* + * Index of the parent power domain node. + * TODO: Figure out whether to whether using pointer is more efficient. + */ + unsigned int parent_node; + + /* + * A CPU power domain does not require state coordination like its + * parent power domains. Hence this node does not include a bakery + * lock. A spinlock is required by the CPU_ON handler to prevent a race + * when multiple CPUs try to turn ON the same target CPU. + */ + spinlock_t cpu_lock; +} cpu_pd_node_t; + +typedef void (*pwrlvl_power_on_finisher_t)(unsigned int cpu_idx, + int max_off_pwrlvl); /******************************************************************************* * Data prototypes ******************************************************************************/ extern const plat_pm_ops_t *psci_plat_pm_ops; -extern pwr_map_node_t psci_pwr_domain_map[PSCI_NUM_PWR_DOMAINS]; -extern pwr_lvl_limits_node_t psci_pwr_domain_map[MPIDR_MAX_AFFLVL + 1]; +extern non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS]; +extern cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT]; extern uint32_t psci_caps; /******************************************************************************* @@ -119,56 +161,47 @@ extern const spd_pm_ops_t *psci_spd_pm; * Function prototypes ******************************************************************************/ /* Private exported functions from psci_common.c */ -unsigned short psci_get_state(pwr_map_node_t *node); -unsigned short psci_get_phys_state(pwr_map_node_t *node); -void psci_set_state(pwr_map_node_t *node, unsigned short state); -unsigned long mpidr_set_pwr_domain_inst(unsigned long, unsigned char, int); +unsigned short psci_get_state(unsigned int idx, int level); +unsigned short psci_get_phys_state(unsigned int idx, int level); +void psci_set_state(unsigned int idx, unsigned short state, int level); int psci_validate_mpidr(unsigned long mpidr); int get_power_on_target_pwrlvl(void); void psci_power_up_finish(int end_pwrlvl, pwrlvl_power_on_finisher_t pon_handler); int psci_get_ns_ep_info(entry_point_info_t *ep, uint64_t entrypoint, uint64_t context_id); -int psci_check_pwrlvl_range(int start_pwrlvl, int end_pwrlvl); -void psci_do_state_coordination(uint32_t start_pwrlvl, - uint32_t end_pwrlvl, - pwr_map_node_t *mpidr_nodes[], - uint32_t state); -void psci_acquire_pwr_domain_locks(int start_pwrlvl, - int end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]); -void psci_release_pwr_domain_locks(int start_pwrlvl, - int end_pwrlvl, - mpidr_pwr_map_nodes_t mpidr_nodes); +void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx, + int end_lvl, + unsigned int node_index[]); +void psci_do_state_coordination(int end_pwrlvl, + unsigned int cpu_idx, + uint32_t state); +void psci_acquire_pwr_domain_locks(int end_pwrlvl, + unsigned int cpu_idx); +void psci_release_pwr_domain_locks(int end_pwrlvl, + unsigned int cpu_idx); void psci_print_power_domain_map(void); -uint32_t psci_find_max_phys_off_pwrlvl(uint32_t start_pwrlvl, - uint32_t end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]); +uint32_t psci_find_max_phys_off_pwrlvl(uint32_t end_pwrlvl, + unsigned int cpu_idx); unsigned int psci_is_last_on_cpu(void); int psci_spd_migrate_info(uint64_t *mpidr); -/* Private exported functions from psci_setup.c */ -int psci_get_pwr_map_nodes(unsigned long mpidr, - int start_pwrlvl, - int end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]); -pwr_map_node_t *psci_get_pwr_map_node(unsigned long, int); - -/* Private exported functions from psci_cpu_on.c */ +/* Private exported functions from psci_on.c */ int psci_cpu_on_start(unsigned long target_cpu, - entry_point_info_t *ep, - int end_pwrlvl); + entry_point_info_t *ep, + int end_pwrlvl); -void psci_cpu_on_finish(pwr_map_node_t *node[], int pwrlvl); +void psci_cpu_on_finish(unsigned int cpu_idx, + int max_off_pwrlvl); /* Private exported functions from psci_cpu_off.c */ int psci_do_cpu_off(int end_pwrlvl); -/* Private exported functions from psci_cpu_suspend.c */ +/* Private exported functions from psci_suspend.c */ void psci_cpu_suspend_start(entry_point_info_t *ep, int end_pwrlvl); - -void psci_cpu_suspend_finish(pwr_map_node_t *node[], int pwrlvl); +void psci_cpu_suspend_finish(unsigned int cpu_idx, + int max_off_pwrlvl); void psci_set_suspend_power_state(unsigned int power_state); diff --git a/services/std_svc/psci1.0/psci_setup.c b/services/std_svc/psci1.0/psci_setup.c index b1eef69f..5f7376be 100644 --- a/services/std_svc/psci1.0/psci_setup.c +++ b/services/std_svc/psci1.0/psci_setup.c @@ -42,335 +42,203 @@ * Per cpu non-secure contexts used to program the architectural state prior * return to the normal world. * TODO: Use the memory allocator to set aside memory for the contexts instead - * of relying on platform defined constants. Using PSCI_NUM_PWR_DOMAINS will be - * an overkill. + * of relying on platform defined constants. ******************************************************************************/ static cpu_context_t psci_ns_context[PLATFORM_CORE_COUNT]; -/******************************************************************************* - * In a system, a certain number of power domain instances are present at a - * power level. The cumulative number of instances across all levels are - * stored in 'psci_pwr_domain_map'. The topology tree has been flattenned into - * this array. To retrieve nodes, information about the extents of each power - * level i.e. start index and end index needs to be present. - * 'psci_pwr_lvl_limits' stores this information. - ******************************************************************************/ -pwr_lvl_limits_node_t psci_pwr_lvl_limits[MPIDR_MAX_AFFLVL + 1]; - /****************************************************************************** * Define the psci capability variable. *****************************************************************************/ uint32_t psci_caps; - -/******************************************************************************* - * Routines for retrieving the node corresponding to a power domain instance - * in the mpidr. The first one uses binary search to find the node corresponding - * to the mpidr (key) at a particular power level. The second routine decides - * extents of the binary search at each power level. - ******************************************************************************/ -static int psci_pwr_domain_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; - - /* - * Make sure we are within array limits. - */ - assert(min_idx >= 0 && max_idx < PSCI_NUM_PWR_DOMAINS); - - /* - * 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_pwr_domain_map' for a given power 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_pwr_domain_map[mid].mpidr > key) - return psci_pwr_domain_map_get_idx(key, min_idx, mid - 1); - else if (psci_pwr_domain_map[mid].mpidr < key) - return psci_pwr_domain_map_get_idx(key, mid + 1, max_idx); - else - return mid; -} - -pwr_map_node_t *psci_get_pwr_map_node(unsigned long mpidr, int pwr_lvl) -{ - int rc; - - if (pwr_lvl > PLAT_MAX_PWR_LVL) - return NULL; - - /* Right shift the mpidr to the required power level */ - mpidr = mpidr_mask_lower_afflvls(mpidr, pwr_lvl); - - rc = psci_pwr_domain_map_get_idx(mpidr, - psci_pwr_lvl_limits[pwr_lvl].min, - psci_pwr_lvl_limits[pwr_lvl].max); - if (rc >= 0) - return &psci_pwr_domain_map[rc]; - else - return NULL; -} - /******************************************************************************* - * This function populates an array with nodes corresponding to a given range of - * power levels in an mpidr. It returns successfully only when the power - * levels are correct, the mpidr is valid i.e. no power level is absent from - * the topology tree & the power domain instance at level 0 is not absent. + * Function which initializes the 'psci_non_cpu_pd_nodes' or the + * 'psci_cpu_pd_nodes' corresponding to the power level. ******************************************************************************/ -int psci_get_pwr_map_nodes(unsigned long mpidr, - int start_pwrlvl, - int end_pwrlvl, - pwr_map_node_t *mpidr_nodes[]) +static void psci_init_pwr_domain_node(int node_idx, int parent_idx, int level) { - int rc = PSCI_E_INVALID_PARAMS, level; - pwr_map_node_t *node; + if (level > PSCI_CPU_PWR_LVL) { + psci_non_cpu_pd_nodes[node_idx].level = level; + psci_lock_init(psci_non_cpu_pd_nodes, node_idx); + psci_non_cpu_pd_nodes[node_idx].parent_node = parent_idx; + } else { - rc = psci_check_pwrlvl_range(start_pwrlvl, end_pwrlvl); - if (rc != PSCI_E_SUCCESS) - return rc; + psci_cpu_pd_nodes[node_idx].parent_node = parent_idx; - for (level = start_pwrlvl; level <= end_pwrlvl; level++) { + /* Initialize with an invalid mpidr */ + psci_cpu_pd_nodes[node_idx].mpidr = PSCI_INVALID_MPIDR; /* - * Grab the node for each power level. No power level - * can be missing as that would mean that the topology tree - * is corrupted. + * Mark the cpu as OFF. */ - node = psci_get_pwr_map_node(mpidr, level); - if (node == NULL) { - rc = PSCI_E_INVALID_PARAMS; - break; - } + set_cpu_data_by_index(node_idx, + psci_svc_cpu_data.psci_state, + PSCI_STATE_OFF); - /* - * Skip absent power levels unless it's power level 0. - * An absent cpu means that the mpidr is invalid. Save the - * pointer to the node for the present power level - */ - if (!(node->state & PSCI_PWR_DOMAIN_PRESENT)) { - if (level == MPIDR_AFFLVL0) { - rc = PSCI_E_INVALID_PARAMS; - break; - } + /* Invalidate the suspend context for the node */ + set_cpu_data_by_index(node_idx, + psci_svc_cpu_data.power_state, + PSCI_INVALID_DATA); - mpidr_nodes[level] = NULL; - } else - mpidr_nodes[level] = node; - } + flush_cpu_data_by_index(node_idx, psci_svc_cpu_data); - return rc; + cm_set_context_by_index(node_idx, + (void *) &psci_ns_context[node_idx], + NON_SECURE); + } } /******************************************************************************* - * Function which initializes the 'pwr_map_node' corresponding to a power - * domain instance. Each node has a unique mpidr, level and bakery lock. - ******************************************************************************/ -static void psci_init_pwr_map_node(unsigned long mpidr, - int level, - unsigned int idx) + * This functions updates cpu_start_idx and ncpus field for each of the node in + * psci_non_cpu_pd_nodes[]. It does so by comparing the parent nodes of each of + * the CPUs and check whether they match with the parent of the previous + * CPU. The basic assumption for this work is that children of the same parent + * are allocated adjacent indices. The platform should ensure this though proper + * mapping of the CPUs to indices via plat_core_pos_by_mpidr() and + * plat_my_core_pos() APIs. + *******************************************************************************/ +static void psci_update_pwrlvl_limits(void) { - unsigned char state; - uint32_t linear_id; - psci_pwr_domain_map[idx].mpidr = mpidr; - psci_pwr_domain_map[idx].level = level; - psci_lock_init(psci_pwr_domain_map, idx); - - /* - * If an power domain instance is present then mark it as OFF - * to begin with. - */ - state = plat_get_pwr_domain_state(level, mpidr); - psci_pwr_domain_map[idx].state = state; - - /* - * Check if this is a CPU node and is present in which case certain - * other initialisations are required. - */ - if (level != MPIDR_AFFLVL0) - return; - - if (!(state & PSCI_PWR_DOMAIN_PRESENT)) - return; - - /* - * Mark the cpu as OFF. Higher power level reference counts - * have already been memset to 0 - */ - psci_set_state(&psci_pwr_domain_map[idx], PSCI_STATE_OFF); - - /* - * Associate a non-secure context with this power - * instance through the context management library. - */ - linear_id = plat_core_pos_by_mpidr(mpidr); - assert(linear_id < PLATFORM_CORE_COUNT); - - /* Invalidate the suspend context for the node */ - set_cpu_data_by_index(linear_id, - psci_svc_cpu_data.power_state, - PSCI_INVALID_DATA); - - flush_cpu_data_by_index(linear_id, psci_svc_cpu_data); - - cm_set_context_by_index(linear_id, - (void *) &psci_ns_context[linear_id], - NON_SECURE); + int cpu_idx, j; + unsigned int nodes_idx[PLAT_MAX_PWR_LVL] = {0}; + unsigned int temp_index[PLAT_MAX_PWR_LVL]; + + for (cpu_idx = 0; cpu_idx < PLATFORM_CORE_COUNT; cpu_idx++) { + psci_get_parent_pwr_domain_nodes(cpu_idx, + PLAT_MAX_PWR_LVL, + temp_index); + for (j = PLAT_MAX_PWR_LVL - 1; j >= 0; j--) { + if (temp_index[j] != nodes_idx[j]) { + nodes_idx[j] = temp_index[j]; + psci_non_cpu_pd_nodes[nodes_idx[j]].cpu_start_idx + = cpu_idx; + } + psci_non_cpu_pd_nodes[nodes_idx[j]].ncpus++; + } + } } /******************************************************************************* - * Core routine used by the Breadth-First-Search algorithm to populate the - * power domain tree. Each level in the tree corresponds to a power level. This - * routine's aim is to traverse to the target power level and populate nodes - * in the 'psci_pwr_domain_map' for all the siblings at that level. It uses the - * current power level to keep track of how many levels from the root of the - * tree have been traversed. If the current power level != target power level, - * then the platform is asked to return the number of children that each - * power domain instance has at the current power level. Traversal is then done - * for each child at the next lower level i.e. current power level - 1. - * - * CAUTION: This routine assumes that power domain instance ids are allocated - * in a monotonically increasing manner at each power level in a mpidr starting - * from 0. If the platform breaks this assumption then this code will have to - * be reworked accordingly. + * Core routine to populate the power domain tree. The tree descriptor passed by + * the platform is populated breadth-first and the first entry in the map + * informs the number of root power domains. The parent nodes of the root nodes + * will point to an invalid entry(-1). ******************************************************************************/ -static unsigned int psci_init_pwr_map(unsigned long mpidr, - unsigned int pwrmap_idx, - int cur_pwrlvl, - int tgt_pwrlvl) +static void populate_power_domain_tree(const unsigned char *topology) { - unsigned int ctr, pwr_inst_count; - - assert(cur_pwrlvl >= tgt_pwrlvl); + unsigned int i, j = 0, num_nodes_at_lvl = 1, num_nodes_at_next_lvl; + unsigned int node_index = 0, parent_node_index = 0, num_children; + int level = PLAT_MAX_PWR_LVL; /* - * Find the number of siblings at the current power level & - * assert if there are none 'cause then we have been invoked with - * an invalid mpidr. + * For each level the inputs are: + * - number of nodes at this level in plat_array i.e. num_nodes_at_level + * This is the sum of values of nodes at the parent level. + * - Index of first entry at this level in the plat_array i.e. + * parent_node_index. + * - Index of first free entry in psci_non_cpu_pd_nodes[] or + * psci_cpu_pd_nodes[] i.e. node_index depending upon the level. */ - pwr_inst_count = plat_get_pwr_domain_count(cur_pwrlvl, mpidr); - assert(pwr_inst_count); - - if (tgt_pwrlvl < cur_pwrlvl) { - for (ctr = 0; ctr < pwr_inst_count; ctr++) { - mpidr = mpidr_set_pwr_domain_inst(mpidr, ctr, - cur_pwrlvl); - pwrmap_idx = psci_init_pwr_map(mpidr, - pwrmap_idx, - cur_pwrlvl - 1, - tgt_pwrlvl); - } - } else { - for (ctr = 0; ctr < pwr_inst_count; ctr++, pwrmap_idx++) { - mpidr = mpidr_set_pwr_domain_inst(mpidr, ctr, - cur_pwrlvl); - psci_init_pwr_map_node(mpidr, cur_pwrlvl, pwrmap_idx); + while (level >= PSCI_CPU_PWR_LVL) { + num_nodes_at_next_lvl = 0; + /* + * For each entry (parent node) at this level in the plat_array: + * - Find the number of children + * - Allocate a node in a power domain array for each child + * - Set the parent of the child to the parent_node_index - 1 + * - Increment parent_node_index to point to the next parent + * - Accumulate the number of children at next level. + */ + for (i = 0; i < num_nodes_at_lvl; i++) { + assert(parent_node_index <= + PSCI_NUM_NON_CPU_PWR_DOMAINS); + num_children = topology[parent_node_index]; + + for (j = node_index; + j < node_index + num_children; j++) + psci_init_pwr_domain_node(j, + parent_node_index - 1, + level); + + node_index = j; + num_nodes_at_next_lvl += num_children; + parent_node_index++; } - /* pwrmap_idx is 1 greater than the max index of cur_pwrlvl */ - psci_pwr_lvl_limits[cur_pwrlvl].max = pwrmap_idx - 1; + num_nodes_at_lvl = num_nodes_at_next_lvl; + level--; + + /* Reset the index for the cpu power domain array */ + if (level == PSCI_CPU_PWR_LVL) + node_index = 0; } - return pwrmap_idx; + /* Validate the sanity of array exported by the platform */ + assert(j == PLATFORM_CORE_COUNT); + +#if !USE_COHERENT_MEM + /* Flush the non CPU power domain data to memory */ + flush_dcache_range((uint64_t) &psci_non_cpu_pd_nodes, + sizeof(psci_non_cpu_pd_nodes)); +#endif } /******************************************************************************* - * This function initializes the topology tree by querying the platform. To do - * so, it's helper routines implement a Breadth-First-Search. At each power - * level the platform conveys the number of power domain instances that exist - * i.e. the power instance count. The algorithm populates the - * psci_pwr_domain_map* recursively using this information. On a platform that - * implements two clusters of 4 cpus each, the populated pwr_map_array would - * look like this: + * This function initializes the power domain topology tree by querying the + * platform. The power domain nodes higher than the CPU are populated in the + * array psci_non_cpu_pd_nodes[] and the CPU power domains are populated in + * psci_cpu_pd_nodes[]. The platform exports its static topology map through the + * populate_power_domain_topology_tree() API. The algorithm populates the + * psci_non_cpu_pd_nodes and psci_cpu_pd_nodes iteratively by using this + * topology map. On a platform that implements two clusters of 2 cpus each, and + * supporting 3 domain levels, the populated psci_non_cpu_pd_nodes would look + * like this: * - * <- cpus cluster0 -><- cpus cluster1 -> * --------------------------------------------------- - * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 | + * | system node | cluster 0 node | cluster 1 node | * --------------------------------------------------- - * ^ ^ - * 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_pwr_lvl_limits' array contains the max & min index of each power - * level within the 'psci_pwr_domain_map' array. This allows restricting search - * of a node at a power level between the indices in the limits array. + * And populated psci_cpu_pd_nodes would look like this : + * <- cpus cluster0 -><- cpus cluster1 -> + * ------------------------------------------------ + * | CPU 0 | CPU 1 | CPU 2 | CPU 3 | + * ------------------------------------------------ ******************************************************************************/ int32_t psci_setup(void) { - unsigned long mpidr = read_mpidr(); - int pwrlvl, pwrmap_idx, max_pwrlvl; - pwr_map_node_t *node; + const unsigned char *topology_tree; - psci_plat_pm_ops = NULL; + /* Query the topology map from the platform */ + topology_tree = plat_get_power_domain_tree_desc(); - /* Find out the maximum power level that the platform implements */ - max_pwrlvl = PLAT_MAX_PWR_LVL; - assert(max_pwrlvl <= MPIDR_MAX_AFFLVL); + /* Populate the power domain arrays using the platform topology map */ + populate_power_domain_tree(topology_tree); - /* - * This call traverses the topology tree with help from the platform and - * populates the power map using a breadth-first-search recursively. - * We assume that the platform allocates power domain instance ids from - * 0 onwards at each power level in the mpidr. FIRST_MPIDR = 0.0.0.0 - */ - pwrmap_idx = 0; - for (pwrlvl = max_pwrlvl; pwrlvl >= MPIDR_AFFLVL0; pwrlvl--) { - pwrmap_idx = psci_init_pwr_map(FIRST_MPIDR, - pwrmap_idx, - max_pwrlvl, - pwrlvl); - } + /* Update the CPU limits for each node in psci_non_cpu_pd_nodes */ + psci_update_pwrlvl_limits(); + + /* Populate the mpidr field of cpu node for this CPU */ + psci_cpu_pd_nodes[plat_my_core_pos()].mpidr = + read_mpidr() & MPIDR_AFFINITY_MASK; #if !USE_COHERENT_MEM /* - * The psci_pwr_domain_map only needs flushing when it's not allocated - * in coherent memory. + * The psci_non_cpu_pd_nodes only needs flushing when it's not allocated in + * coherent memory. */ - flush_dcache_range((uint64_t) &psci_pwr_domain_map, - sizeof(psci_pwr_domain_map)); + flush_dcache_range((uint64_t) &psci_non_cpu_pd_nodes, + sizeof(psci_non_cpu_pd_nodes)); #endif - /* - * Set the bounds for number of instances of each level in the map. Also - * flush out the entire array so that it's visible to subsequent power - * management operations. The 'psci_pwr_lvl_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 (pwrlvl = MPIDR_AFFLVL0; pwrlvl < max_pwrlvl; pwrlvl++) { - psci_pwr_lvl_limits[pwrlvl].min = - psci_pwr_lvl_limits[pwrlvl + 1].max + 1; - } - - flush_dcache_range((unsigned long) psci_pwr_lvl_limits, - sizeof(psci_pwr_lvl_limits)); + flush_dcache_range((uint64_t) &psci_cpu_pd_nodes, + sizeof(psci_cpu_pd_nodes)); /* - * Mark the power domain instances in our mpidr as ON. No need to lock - * as this is the primary cpu. + * Mark the current CPU and its parent power domains as ON. No need to lock + * as the system is UP on the primary at this stage of boot. */ - mpidr &= MPIDR_AFFINITY_MASK; - for (pwrlvl = MPIDR_AFFLVL0; pwrlvl <= max_pwrlvl; pwrlvl++) { - - node = psci_get_pwr_map_node(mpidr, pwrlvl); - assert(node); - - /* Mark each present node as ON. */ - if (node->state & PSCI_PWR_DOMAIN_PRESENT) - psci_set_state(node, PSCI_STATE_ON); - } + psci_do_state_coordination(PLAT_MAX_PWR_LVL, plat_my_core_pos(), + PSCI_STATE_ON); platform_setup_pm(&psci_plat_pm_ops); assert(psci_plat_pm_ops); diff --git a/services/std_svc/psci1.0/psci_suspend.c b/services/std_svc/psci1.0/psci_suspend.c index 3d1bf09e..c402937a 100644 --- a/services/std_svc/psci1.0/psci_suspend.c +++ b/services/std_svc/psci1.0/psci_suspend.c @@ -82,15 +82,15 @@ int psci_get_suspend_stateid(void) } /******************************************************************************* - * This function gets the state id of the cpu specified by the 'mpidr' parameter + * This function gets the state id of the cpu specified by the cpu index * from the power state parameter saved in the per-cpu data array. Returns * PSCI_INVALID_DATA if the power state saved is invalid. ******************************************************************************/ -int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr) +int psci_get_suspend_stateid_by_idx(unsigned long cpu_idx) { unsigned int power_state; - power_state = get_cpu_data_by_index(plat_core_pos_by_mpidr(mpidr), + power_state = get_cpu_data_by_index(cpu_idx, psci_svc_cpu_data.power_state); return ((power_state == PSCI_INVALID_DATA) ? @@ -114,12 +114,10 @@ int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr) * the state transition has been done, no further error is expected and it is * not possible to undo any of the actions taken beyond that point. ******************************************************************************/ -void psci_cpu_suspend_start(entry_point_info_t *ep, - int end_pwrlvl) +void psci_cpu_suspend_start(entry_point_info_t *ep, int end_pwrlvl) { int skip_wfi = 0; - mpidr_pwr_map_nodes_t mpidr_nodes; - unsigned int max_phys_off_pwrlvl; + unsigned int max_phys_off_pwrlvl, idx = plat_my_core_pos(); unsigned long psci_entrypoint; /* @@ -130,24 +128,12 @@ void psci_cpu_suspend_start(entry_point_info_t *ep, psci_plat_pm_ops->pwr_domain_suspend_finish); /* - * Collect the pointers to the nodes in the topology tree for - * each power domain instance in the mpidr. If this function does - * not return successfully then either the mpidr or the power - * levels are incorrect. Either way, this an internal TF error - * therefore assert. - */ - if (psci_get_pwr_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0, end_pwrlvl, mpidr_nodes) != PSCI_E_SUCCESS) - assert(0); - - /* * This function acquires the lock corresponding to each power * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + psci_acquire_pwr_domain_locks(end_pwrlvl, + idx); /* * We check if there are any pending interrupts after the delay @@ -169,17 +155,15 @@ void psci_cpu_suspend_start(entry_point_info_t *ep, /* * This function updates the state of each power domain instance - * corresponding to the mpidr in the range of power levels + * corresponding to the cpu index in the range of power levels * specified. */ - psci_do_state_coordination(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes, - PSCI_STATE_SUSPEND); - - max_phys_off_pwrlvl = psci_find_max_phys_off_pwrlvl(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + psci_do_state_coordination(end_pwrlvl, + idx, + PSCI_STATE_SUSPEND); + + max_phys_off_pwrlvl = psci_find_max_phys_off_pwrlvl(end_pwrlvl, + idx); assert(max_phys_off_pwrlvl != PSCI_INVALID_DATA); /* @@ -210,9 +194,8 @@ exit: * Release the locks corresponding to each power level in the * reverse order to which they were acquired. */ - psci_release_pwr_domain_locks(MPIDR_AFFLVL0, - end_pwrlvl, - mpidr_nodes); + psci_release_pwr_domain_locks(end_pwrlvl, + idx); if (!skip_wfi) psci_power_down_wfi(); } @@ -221,15 +204,14 @@ exit: * The following functions finish an earlier suspend request. They * are called by the common finisher routine in psci_common.c. ******************************************************************************/ -void psci_cpu_suspend_finish(pwr_map_node_t *node[], int pwrlvl) +void psci_cpu_suspend_finish(unsigned int cpu_idx, int max_off_pwrlvl) { int32_t suspend_level; uint64_t counter_freq; - assert(node[pwrlvl]->level == pwrlvl); - /* Ensure we have been woken up from a suspended state */ - assert(psci_get_state(node[MPIDR_AFFLVL0]) == PSCI_STATE_SUSPEND); + assert(psci_get_state(cpu_idx, PSCI_CPU_PWR_LVL) + == PSCI_STATE_SUSPEND); /* * Plat. management: Perform the platform specific actions @@ -238,7 +220,7 @@ void psci_cpu_suspend_finish(pwr_map_node_t *node[], int pwrlvl) * wrong then assert as there is no way to recover from this * situation. */ - psci_plat_pm_ops->pwr_domain_suspend_finish(pwrlvl); + psci_plat_pm_ops->pwr_domain_suspend_finish(max_off_pwrlvl); /* * Arch. management: Enable the data cache, manage stack memory and @@ -275,4 +257,3 @@ void psci_cpu_suspend_finish(pwr_map_node_t *node[], int pwrlvl) /* Clean caches before re-entering normal world */ dcsw_op_louis(DCCSW); } - |