diff options
Diffstat (limited to 'common')
| -rw-r--r-- | common/psci/psci_afflvl_off.c | 176 | ||||
| -rw-r--r-- | common/psci/psci_afflvl_on.c | 182 | ||||
| -rw-r--r-- | common/psci/psci_afflvl_suspend.c | 228 | ||||
| -rw-r--r-- | common/psci/psci_common.c | 251 | ||||
| -rw-r--r-- | common/psci/psci_main.c | 24 | ||||
| -rw-r--r-- | common/psci/psci_private.h | 22 | ||||
| -rw-r--r-- | common/psci/psci_setup.c | 50 |
7 files changed, 601 insertions, 332 deletions
diff --git a/common/psci/psci_afflvl_off.c b/common/psci/psci_afflvl_off.c index b62ae29..22685ba 100644 --- a/common/psci/psci_afflvl_off.c +++ b/common/psci/psci_afflvl_off.c @@ -162,97 +162,135 @@ static const afflvl_off_handler psci_afflvl_off_handlers[] = { }; /******************************************************************************* - * This function implements the core of the processing required to turn a cpu - * off. It's assumed that along with turning the cpu off, higher affinity levels - * will be turned off as far as possible. We first need to determine the new - * state off all the affinity instances in the mpidr corresponding to the target - * cpu. Action will be taken on the basis of this new state. To do the state - * change we first need to acquire the locks for all the implemented affinity - * level to be able to snapshot the system state. Then we need to start turning - * affinity levels off from the lowest to the highest (e.g. a cpu needs to be - * off before a cluster can be turned off). To achieve this flow, we start - * acquiring the locks from the highest to the lowest affinity level. Once we - * reach affinity level 0, we do the state change followed by the actions - * corresponding to the new state for affinity level 0. Actions as per the - * updated state for higher affinity levels are performed as we unwind back to - * highest affinity level. + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the off handler for the corresponding affinity + * levels + ******************************************************************************/ +static int psci_call_off_handlers(mpidr_aff_map_nodes mpidr_nodes, + int start_afflvl, + int end_afflvl, + unsigned long mpidr) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node *node; + + for (level = start_afflvl; level <= end_afflvl; level++) { + node = mpidr_nodes[level]; + if (node == NULL) + continue; + + /* + * TODO: In case of an error should there be a way + * of restoring what we might have torn down at + * lower affinity levels. + */ + rc = psci_afflvl_off_handlers[level](mpidr, node); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Top level handler which is called when a cpu wants to power itself down. + * It's assumed that along with turning the cpu off, higher affinity levels will + * be turned off as far as possible. It traverses through all the affinity + * levels performing generic, architectural, platform setup and state management + * e.g. for a cluster that's to be powered off, it will call the platform + * specific code which will disable coherency at the interconnect level if the + * cpu is the last in the cluster. For a cpu it could mean programming the power + * the power controller etc. + * + * The state of all the relevant affinity levels is changed prior to calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is about to enter. + * + * The affinity level specific handlers are called in ascending order i.e. from + * the lowest to the highest affinity level implemented by the platform because + * to turn off affinity level X it is neccesary to turn off affinity level X - 1 + * first. + * + * CAUTION: This function is called with coherent stacks so that coherency can + * be turned off and caches can be flushed safely. ******************************************************************************/ int psci_afflvl_off(unsigned long mpidr, - int cur_afflvl, - int tgt_afflvl) + int start_afflvl, + int end_afflvl) { - int rc = PSCI_E_SUCCESS, level; - unsigned int next_state, prev_state; - aff_map_node *aff_node; + int rc = PSCI_E_SUCCESS; + unsigned int prev_state; + mpidr_aff_map_nodes mpidr_nodes; mpidr &= MPIDR_AFFINITY_MASK;; /* - * Some affinity instances at levels between the current and - * target levels could be absent in the mpidr. Skip them and - * start from the first present instance. + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. In either case, we cannot return back + * to the caller as it would not know what to do. */ - level = psci_get_first_present_afflvl(mpidr, - cur_afflvl, - tgt_afflvl, - &aff_node); + rc = psci_get_aff_map_nodes(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + assert (rc == PSCI_E_SUCCESS); + /* - * Return if there are no more affinity instances beyond this - * level to process. Else ensure that the returned affinity - * node makes sense. + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. */ - if (aff_node == NULL) - return rc; - - assert(level == aff_node->level); + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); /* - * This function acquires the lock corresponding to each - * affinity level so that state management can be done safely. + * Keep the old cpu state handy. It will be used to restore the + * system to its original state in case something goes wrong */ - bakery_lock_get(mpidr, &aff_node->lock); - - /* Keep the old state and the next one handy */ - prev_state = psci_get_state(aff_node->state); - next_state = PSCI_STATE_OFF; + prev_state = psci_get_state(mpidr_nodes[MPIDR_AFFLVL0]->state); /* - * We start from the highest affinity level and work our way - * downwards to the lowest i.e. MPIDR_AFFLVL0. + * State management: Update the state of each affinity instance + * between the start and end affinity levels */ - if (aff_node->level == tgt_afflvl) { - psci_change_state(mpidr, - tgt_afflvl, - get_max_afflvl(), - next_state); - } else { - rc = psci_afflvl_off(mpidr, level - 1, tgt_afflvl); - if (rc != PSCI_E_SUCCESS) { - psci_set_state(aff_node->state, prev_state); - goto exit; - } - } + psci_change_state(mpidr_nodes, + start_afflvl, + end_afflvl, + PSCI_STATE_OFF); + + /* Perform generic, architecture and platform specific handling */ + rc = psci_call_off_handlers(mpidr_nodes, + start_afflvl, + end_afflvl, + mpidr); /* - * Perform generic, architecture and platform specific - * handling + * If an error is returned by a handler then restore the cpu state + * to its original value. If the cpu state is restored then that + * should result in the state of the higher affinity levels to + * get restored as well. + * TODO: We are not undoing any architectural or platform specific + * operations that might have completed before encountering the + * error. The system might not be in a stable state. */ - rc = psci_afflvl_off_handlers[level](mpidr, aff_node); - if (rc != PSCI_E_SUCCESS) { - psci_set_state(aff_node->state, prev_state); - goto exit; - } + if (rc != PSCI_E_SUCCESS) + psci_change_state(mpidr_nodes, + start_afflvl, + end_afflvl, + prev_state); /* - * If all has gone as per plan then this cpu should be - * marked as OFF + * Release the locks corresponding to each affinity level in the + * reverse order to which they were acquired. */ - if (level == MPIDR_AFFLVL0) { - next_state = psci_get_state(aff_node->state); - assert(next_state == PSCI_STATE_OFF); - } + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); -exit: - bakery_lock_release(mpidr, &aff_node->lock); return rc; } diff --git a/common/psci/psci_afflvl_on.c b/common/psci/psci_afflvl_on.c index 81d46bf..c9c3b2c 100644 --- a/common/psci/psci_afflvl_on.c +++ b/common/psci/psci_afflvl_on.c @@ -207,84 +207,119 @@ static const afflvl_on_handler psci_afflvl_on_handlers[] = { }; /******************************************************************************* - * This function implements the core of the processing required to turn a cpu - * on. It avoids recursion to traverse from the lowest to the highest affinity - * level unlike the off/suspend/pon_finisher functions. It does ensure that the - * locks are picked in the same order as the order routines to avoid deadlocks. - * The flow is: Take all the locks until the highest affinity level, Call the - * handlers for turning an affinity level on & finally change the state of the - * affinity level. + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the on handler for the corresponding affinity + * levels + ******************************************************************************/ +static int psci_call_on_handlers(mpidr_aff_map_nodes target_cpu_nodes, + int start_afflvl, + int end_afflvl, + unsigned long target_cpu, + unsigned long entrypoint, + unsigned long context_id) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node *node; + + for (level = end_afflvl; level >= start_afflvl; level--) { + node = target_cpu_nodes[level]; + if (node == NULL) + continue; + + /* + * TODO: In case of an error should there be a way + * of undoing what we might have setup at higher + * affinity levels. + */ + rc = psci_afflvl_on_handlers[level](target_cpu, + node, + entrypoint, + context_id); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Generic handler which is called to physically power on a cpu identified by + * its mpidr. It traverses through all the affinity levels performing generic, + * architectural, platform setup and state management e.g. for a cpu that is + * to be powered on, it will ensure that enough information is stashed for it + * to resume execution in the non-secure security state. + * + * The state of all the relevant affinity levels is changed after calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is currently in. + * + * The affinity level specific handlers are called in descending order i.e. from + * the highest to the lowest affinity level implemented by the platform because + * to turn on affinity level X it is neccesary to turn on affinity level X + 1 + * first. ******************************************************************************/ int psci_afflvl_on(unsigned long target_cpu, unsigned long entrypoint, unsigned long context_id, - int current_afflvl, - int target_afflvl) + int start_afflvl, + int end_afflvl) { - unsigned int prev_state, next_state; - int rc = PSCI_E_SUCCESS, level; - aff_map_node *aff_node; + int rc = PSCI_E_SUCCESS; + mpidr_aff_map_nodes target_cpu_nodes; unsigned long mpidr = read_mpidr() & MPIDR_AFFINITY_MASK; /* - * This loop acquires the lock corresponding to each - * affinity level so that by the time we hit the lowest - * affinity level, the system topology is snapshot and - * state management can be done safely. + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. */ - for (level = current_afflvl; level >= target_afflvl; level--) { - aff_node = psci_get_aff_map_node(target_cpu, level); - if (aff_node) - bakery_lock_get(mpidr, &aff_node->lock); - } + rc = psci_get_aff_map_nodes(target_cpu, + start_afflvl, + end_afflvl, + target_cpu_nodes); + if (rc != PSCI_E_SUCCESS) + return rc; + /* - * Perform generic, architecture and platform specific - * handling + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. */ - for (level = current_afflvl; level >= target_afflvl; level--) { - - /* Grab the node for each affinity level once again */ - aff_node = psci_get_aff_map_node(target_cpu, level); - if (aff_node) { - - /* Keep the old state and the next one handy */ - prev_state = psci_get_state(aff_node->state); - rc = psci_afflvl_on_handlers[level](target_cpu, - aff_node, - entrypoint, - context_id); - if (rc != PSCI_E_SUCCESS) { - psci_set_state(aff_node->state, prev_state); - goto exit; - } - } - } + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + target_cpu_nodes); + + /* Perform generic, architecture and platform specific handling. */ + rc = psci_call_on_handlers(target_cpu_nodes, + start_afflvl, + end_afflvl, + target_cpu, + entrypoint, + context_id); + if (rc != PSCI_E_SUCCESS) + goto exit; /* - * State management: Update the states since this is the - * target affinity level requested. + * State management: Update the state of each affinity instance + * between the start and end affinity levels */ - psci_change_state(target_cpu, - target_afflvl, - get_max_afflvl(), + psci_change_state(target_cpu_nodes, + start_afflvl, + end_afflvl, PSCI_STATE_ON_PENDING); exit: /* * This loop releases the lock corresponding to each affinity level - * in the reverse order. It also checks the final state of the cpu. + * in the reverse order to which they were acquired. */ - for (level = target_afflvl; level <= current_afflvl; level++) { - aff_node = psci_get_aff_map_node(target_cpu, level); - if (aff_node) { - if (level == MPIDR_AFFLVL0) { - next_state = psci_get_state(aff_node->state); - assert(next_state == PSCI_STATE_ON_PENDING); - } - bakery_lock_release(mpidr, &aff_node->lock); - } - } + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + target_cpu_nodes); return rc; } @@ -294,13 +329,16 @@ exit: * are called by the common finisher routine in psci_common.c. ******************************************************************************/ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr, - aff_map_node *cpu_node, - unsigned int prev_state) + aff_map_node *cpu_node) { - unsigned int index, plat_state, rc = PSCI_E_SUCCESS; + unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS; assert(cpu_node->level == MPIDR_AFFLVL0); + /* Ensure we have been explicitly woken up by another cpu */ + state = psci_get_state(cpu_node->state); + assert(state == PSCI_STATE_ON_PENDING); + /* * Plat. management: Perform the platform specific actions * for this cpu e.g. enabling the gic or zeroing the mailbox @@ -309,8 +347,8 @@ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr, */ if (psci_plat_pm_ops->affinst_on_finish) { - /* Get the previous physical state of this cpu */ - plat_state = psci_get_phys_state(prev_state); + /* Get the physical state of this cpu */ + plat_state = psci_get_phys_state(state); rc = psci_plat_pm_ops->affinst_on_finish(mpidr, cpu_node->level, plat_state); @@ -346,11 +384,9 @@ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr, } static unsigned int psci_afflvl1_on_finish(unsigned long mpidr, - aff_map_node *cluster_node, - unsigned int prev_state) + aff_map_node *cluster_node) { - unsigned int rc = PSCI_E_SUCCESS; - unsigned int plat_state; + unsigned int plat_state, rc = PSCI_E_SUCCESS; assert(cluster_node->level == MPIDR_AFFLVL1); @@ -363,7 +399,9 @@ static unsigned int psci_afflvl1_on_finish(unsigned long mpidr, * situation. */ if (psci_plat_pm_ops->affinst_on_finish) { - plat_state = psci_get_phys_state(prev_state); + + /* Get the physical state of this cluster */ + plat_state = psci_get_aff_phys_state(cluster_node); rc = psci_plat_pm_ops->affinst_on_finish(mpidr, cluster_node->level, plat_state); @@ -375,11 +413,9 @@ static unsigned int psci_afflvl1_on_finish(unsigned long mpidr, static unsigned int psci_afflvl2_on_finish(unsigned long mpidr, - aff_map_node *system_node, - unsigned int prev_state) + aff_map_node *system_node) { - int rc = PSCI_E_SUCCESS; - unsigned int plat_state; + unsigned int plat_state, rc = PSCI_E_SUCCESS; /* Cannot go beyond this affinity level */ assert(system_node->level == MPIDR_AFFLVL2); @@ -398,7 +434,9 @@ static unsigned int psci_afflvl2_on_finish(unsigned long mpidr, * situation. */ if (psci_plat_pm_ops->affinst_on_finish) { - plat_state = psci_get_phys_state(system_node->state); + + /* Get the physical state of the system */ + plat_state = psci_get_aff_phys_state(system_node); rc = psci_plat_pm_ops->affinst_on_finish(mpidr, system_node->level, plat_state); diff --git a/common/psci/psci_afflvl_suspend.c b/common/psci/psci_afflvl_suspend.c index 810075b..186f048 100644 --- a/common/psci/psci_afflvl_suspend.c +++ b/common/psci/psci_afflvl_suspend.c @@ -226,112 +226,149 @@ static const afflvl_suspend_handler psci_afflvl_suspend_handlers[] = { }; /******************************************************************************* - * This function implements the core of the processing required to suspend a cpu - * It'S assumed that along with suspending the cpu, higher affinity levels will - * be suspended as far as possible. Suspending a cpu is equivalent to physically - * powering it down, but the cpu is still available to the OS for scheduling. - * We first need to determine the new state off all the affinity instances in - * the mpidr corresponding to the target cpu. Action will be taken on the basis - * of this new state. To do the state change we first need to acquire the locks - * for all the implemented affinity level to be able to snapshot the system - * state. Then we need to start suspending affinity levels from the lowest to - * the highest (e.g. a cpu needs to be suspended before a cluster can be). To - * achieve this flow, we start acquiring the locks from the highest to the - * lowest affinity level. Once we reach affinity level 0, we do the state change - * followed by the actions corresponding to the new state for affinity level 0. - * Actions as per the updated state for higher affinity levels are performed as - * we unwind back to highest affinity level. + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the suspend handler for the corresponding affinity + * levels + ******************************************************************************/ +static int psci_call_suspend_handlers(mpidr_aff_map_nodes mpidr_nodes, + int start_afflvl, + int end_afflvl, + unsigned long mpidr, + unsigned long entrypoint, + unsigned long context_id, + unsigned int power_state) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node *node; + + for (level = start_afflvl; level <= end_afflvl; level++) { + node = mpidr_nodes[level]; + if (node == NULL) + continue; + + /* + * TODO: In case of an error should there be a way + * of restoring what we might have torn down at + * lower affinity levels. + */ + rc = psci_afflvl_suspend_handlers[level](mpidr, + node, + entrypoint, + context_id, + power_state); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Top level handler which is called when a cpu wants to suspend its execution. + * It is assumed that along with turning the cpu off, higher affinity levels + * until the target affinity level will be turned off as well. It traverses + * through all the affinity levels performing generic, architectural, platform + * setup and state management e.g. for a cluster that's to be suspended, it will + * call the platform specific code which will disable coherency at the + * interconnect level if the cpu is the last in the cluster. For a cpu it could + * mean programming the power controller etc. + * + * The state of all the relevant affinity levels is changed prior to calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is about to enter. + * + * The affinity level specific handlers are called in ascending order i.e. from + * the lowest to the highest affinity level implemented by the platform because + * to turn off affinity level X it is neccesary to turn off affinity level X - 1 + * first. + * + * CAUTION: This function is called with coherent stacks so that coherency can + * be turned off and caches can be flushed safely. ******************************************************************************/ int psci_afflvl_suspend(unsigned long mpidr, unsigned long entrypoint, unsigned long context_id, unsigned int power_state, - int cur_afflvl, - int tgt_afflvl) + int start_afflvl, + int end_afflvl) { - int rc = PSCI_E_SUCCESS, level; - unsigned int prev_state, next_state; - aff_map_node *aff_node; + int rc = PSCI_E_SUCCESS; + unsigned int prev_state; + mpidr_aff_map_nodes mpidr_nodes; mpidr &= MPIDR_AFFINITY_MASK; /* - * Some affinity instances at levels between the current and - * target levels could be absent in the mpidr. Skip them and - * start from the first present instance. + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. */ - level = psci_get_first_present_afflvl(mpidr, - cur_afflvl, - tgt_afflvl, - &aff_node); + rc = psci_get_aff_map_nodes(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + if (rc != PSCI_E_SUCCESS) + return rc; /* - * Return if there are no more affinity instances beyond this - * level to process. Else ensure that the returned affinity - * node makes sense. + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. */ - if (aff_node == NULL) - return rc; - - assert(level == aff_node->level); + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); /* - * This function acquires the lock corresponding to each - * affinity level so that state management can be done safely. + * Keep the old cpu state handy. It will be used to restore the + * system to its original state in case something goes wrong */ - bakery_lock_get(mpidr, &aff_node->lock); - - /* Keep the old state and the next one handy */ - prev_state = psci_get_state(aff_node->state); - next_state = PSCI_STATE_SUSPEND; + prev_state = psci_get_state(mpidr_nodes[MPIDR_AFFLVL0]->state); /* - * We start from the highest affinity level and work our way - * downwards to the lowest i.e. MPIDR_AFFLVL0. + * State management: Update the state of each affinity instance + * between the start and end affinity levels */ - if (aff_node->level == tgt_afflvl) { - psci_change_state(mpidr, - tgt_afflvl, - get_max_afflvl(), - next_state); - } else { - rc = psci_afflvl_suspend(mpidr, - entrypoint, - context_id, - power_state, - level - 1, - tgt_afflvl); - if (rc != PSCI_E_SUCCESS) { - psci_set_state(aff_node->state, prev_state); - goto exit; - } - } + psci_change_state(mpidr_nodes, + start_afflvl, + end_afflvl, + PSCI_STATE_SUSPEND); + + /* Perform generic, architecture and platform specific handling */ + rc = psci_call_suspend_handlers(mpidr_nodes, + start_afflvl, + end_afflvl, + mpidr, + entrypoint, + context_id, + power_state); /* - * Perform generic, architecture and platform specific - * handling + * If an error is returned by a handler then restore the cpu state + * to its original value. If the cpu state is restored then that + * should result in the state of the higher affinity levels to + * get restored as well. + * TODO: We are not undoing any architectural or platform specific + * operations that might have completed before encountering the + * error. The system might not be in a stable state. */ - rc = psci_afflvl_suspend_handlers[level](mpidr, - aff_node, - entrypoint, - context_id, - power_state); - if (rc != PSCI_E_SUCCESS) { - psci_set_state(aff_node->state, prev_state); - goto exit; - } + if (rc != PSCI_E_SUCCESS) + psci_change_state(mpidr_nodes, + start_afflvl, + end_afflvl, + prev_state); /* - * If all has gone as per plan then this cpu should be - * marked as OFF + * Release the locks corresponding to each affinity level in the + * reverse order to which they were acquired. */ - if (level == MPIDR_AFFLVL0) { - next_state = psci_get_state(aff_node->state); - assert(next_state == PSCI_STATE_SUSPEND); - } + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); -exit: - bakery_lock_release(mpidr, &aff_node->lock); return rc; } @@ -340,13 +377,16 @@ exit: * are called by the common finisher routine in psci_common.c. ******************************************************************************/ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, - aff_map_node *cpu_node, - unsigned int prev_state) + aff_map_node *cpu_node) { - unsigned int index, plat_state, rc = 0; + unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS; assert(cpu_node->level == MPIDR_AFFLVL0); + /* Ensure we have been woken up from a suspended state */ + state = psci_get_state(cpu_node->state); + assert(state == PSCI_STATE_SUSPEND); + /* * Plat. management: Perform the platform specific actions * before we change the state of the cpu e.g. enabling the @@ -355,7 +395,9 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, * situation. */ if (psci_plat_pm_ops->affinst_suspend_finish) { - plat_state = psci_get_phys_state(prev_state); + + /* Get the physical state of this cpu */ + plat_state = psci_get_phys_state(state); rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, cpu_node->level, plat_state); @@ -396,11 +438,9 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, } static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr, - aff_map_node *cluster_node, - unsigned int prev_state) + aff_map_node *cluster_node) { - unsigned int rc = 0; - unsigned int plat_state; + unsigned int plat_state, rc = PSCI_E_SUCCESS; assert(cluster_node->level == MPIDR_AFFLVL1); @@ -413,7 +453,9 @@ static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr, * situation. */ if (psci_plat_pm_ops->affinst_suspend_finish) { - plat_state = psci_get_phys_state(prev_state); + + /* Get the physical state of this cpu */ + plat_state = psci_get_aff_phys_state(cluster_node); rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, cluster_node->level, plat_state); @@ -425,11 +467,9 @@ static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr, static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr, - aff_map_node *system_node, - unsigned int target_afflvl) + aff_map_node *system_node) { - int rc = PSCI_E_SUCCESS; - unsigned int plat_state; + unsigned int plat_state, rc = PSCI_E_SUCCESS;; /* Cannot go beyond this affinity level */ assert(system_node->level == MPIDR_AFFLVL2); @@ -448,7 +488,9 @@ static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr, * situation. */ if (psci_plat_pm_ops->affinst_suspend_finish) { - plat_state = psci_get_phys_state(system_node->state); + + /* Get the physical state of the system */ + plat_state = psci_get_aff_phys_state(system_node); rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, system_node->level, plat_state); diff --git a/common/psci/psci_common.c b/common/psci/psci_common.c index ba0a379..c37658b 100644 --- a/common/psci/psci_common.c +++ b/common/psci/psci_common.c @@ -111,6 +111,62 @@ unsigned long mpidr_set_aff_inst(unsigned long mpidr, } /******************************************************************************* + * This function sanity checks a range of affinity levels. + ******************************************************************************/ +int psci_check_afflvl_range(int start_afflvl, int end_afflvl) +{ + /* Sanity check the parameters passed */ + if (end_afflvl > MPIDR_MAX_AFFLVL) + return PSCI_E_INVALID_PARAMS; + + if (start_afflvl < MPIDR_AFFLVL0) + return PSCI_E_INVALID_PARAMS; + + if (end_afflvl < start_afflvl) + return PSCI_E_INVALID_PARAMS; + + return PSCI_E_SUCCESS; +} + +/******************************************************************************* + * This function is passed an array of pointers to affinity level nodes in the + * topology tree for an mpidr. It picks up locks for each affinity level bottom + * up in the range specified. + ******************************************************************************/ +void psci_acquire_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes mpidr_nodes) +{ + int level; + + for (level = start_afflvl; level <= end_afflvl; level++) { + if (mpidr_nodes[level] == NULL) + continue; + bakery_lock_get(mpidr, &mpidr_nodes[level]->lock); + } +} + +/******************************************************************************* + * This function is passed an array of pointers to affinity level nodes in the + * topology tree for an mpidr. It releases the lock for each affinity level top + * down in the range specified. + ******************************************************************************/ +void psci_release_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes mpidr_nodes) +{ + int level; + + for (level = end_afflvl; level >= start_afflvl; level--) { + if (mpidr_nodes[level] == NULL) + continue; + bakery_lock_release(mpidr, &mpidr_nodes[level]->lock); + } +} + +/******************************************************************************* * Simple routine to determine whether an affinity instance at a given level * in an mpidr exists or not. ******************************************************************************/ @@ -158,37 +214,44 @@ int psci_get_first_present_afflvl(unsigned long mpidr, } /******************************************************************************* - * Recursively change the affinity state between the current and target affinity + * Iteratively change the affinity state between the current and target affinity * levels. The target state matters only if we are starting from affinity level * 0 i.e. a cpu otherwise the state depends upon the state of the lower affinity * levels. ******************************************************************************/ -int psci_change_state(unsigned long mpidr, - int cur_afflvl, - int tgt_afflvl, +int psci_change_state(mpidr_aff_map_nodes mpidr_nodes, + int start_afflvl, + int end_afflvl, unsigned int tgt_state) { - int rc = PSCI_E_SUCCESS; + int rc = PSCI_E_SUCCESS, level; unsigned int state; - aff_map_node *aff_node; - - /* Sanity check the affinity levels */ - assert(tgt_afflvl >= cur_afflvl); - - aff_node = psci_get_aff_map_node(mpidr, cur_afflvl); - assert(aff_node); + aff_map_node *node; - /* TODO: Check whether the affinity level is present or absent*/ + /* + * Get a temp pointer to the node. It is not possible that affinity + * level 0 is missing. Simply ignore higher missing levels. + */ + for (level = start_afflvl; level <= end_afflvl; level++) { - if (cur_afflvl == MPIDR_AFFLVL0) { - psci_set_state(aff_node->state, tgt_state); - } else { - state = psci_calculate_affinity_state(aff_node); - psci_set_state(aff_node->state, state); + node = mpidr_nodes[level]; + if (level == MPIDR_AFFLVL0) { + assert(node); + psci_set_state(node->state, tgt_state); + } else { + if (node == NULL) + continue; + state = psci_calculate_affinity_state(node); + psci_set_state(node->state, state); + } } - if (cur_afflvl != tgt_afflvl) - psci_change_state(mpidr, cur_afflvl + 1, tgt_afflvl, tgt_state); + /* If all went well then the cpu should be in the target state */ + if (start_afflvl == MPIDR_AFFLVL0) { + node = mpidr_nodes[MPIDR_AFFLVL0]; + state = psci_get_state(node->state); + assert(tgt_state == state); + } return rc; } @@ -443,92 +506,112 @@ unsigned int psci_get_aff_phys_state(aff_map_node *aff_node) } /******************************************************************************* + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the physical power on handler for the corresponding + * affinity levels + ******************************************************************************/ +static int psci_call_power_on_handlers(mpidr_aff_map_nodes mpidr_nodes, + int start_afflvl, + int end_afflvl, + afflvl_power_on_finisher *pon_handlers, + unsigned long mpidr) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node *node; + + for (level = end_afflvl; level >= start_afflvl; level--) { + node = mpidr_nodes[level]; + if (node == NULL) + continue; + + /* + * If we run into any trouble while powering up an + * affinity instance, then there is no recovery path + * so simply return an error and let the caller take + * care of the situation. + */ + rc = pon_handlers[level](mpidr, node); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* * Generic handler which is called when a cpu is physically powered on. It - * recurses through all the affinity levels performing generic, architectural, + * traverses through all the affinity levels performing generic, architectural, * platform setup and state management e.g. for a cluster that's been powered * on, it will call the platform specific code which will enable coherency at * the interconnect level. For a cpu it could mean turning on the MMU etc. * - * This function traverses from the lowest to the highest affinity level - * implemented by the platform. Since it's recursive, for each call the - * 'cur_afflvl' & 'tgt_afflvl' parameters keep track of which level we are at - * and which level we need to get to respectively. Locks are picked up along the - * way so that when the lowest affinity level is hit, state management can be - * safely done. Prior to this, each affinity level does it's bookeeping as per - * the state out of reset. + * The state of all the relevant affinity levels is changed after calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is exiting from. + * + * The affinity level specific handlers are called in descending order i.e. from + * the highest to the lowest affinity level implemented by the platform because + * to turn on affinity level X it is neccesary to turn on affinity level X + 1 + * first. * * CAUTION: This function is called with coherent stacks so that coherency and * the mmu can be turned on safely. ******************************************************************************/ -unsigned int psci_afflvl_power_on_finish(unsigned long mpidr, - int cur_afflvl, - int tgt_afflvl, - afflvl_power_on_finisher *pon_handlers) +void psci_afflvl_power_on_finish(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + afflvl_power_on_finisher *pon_handlers) { - unsigned int prev_state, next_state, rc = PSCI_E_SUCCESS; - aff_map_node *aff_node; - int level; + mpidr_aff_map_nodes mpidr_nodes; + int rc; mpidr &= MPIDR_AFFINITY_MASK;; /* - * Some affinity instances at levels between the current and - * target levels could be absent in the mpidr. Skip them and - * start from the first present instance. + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. Either case is an irrecoverable error. */ - level = psci_get_first_present_afflvl(mpidr, - cur_afflvl, - tgt_afflvl, - &aff_node); - /* - * Return if there are no more affinity instances beyond this - * level to process. Else ensure that the returned affinity - * node makes sense. - */ - if (aff_node == NULL) - return rc; - - assert(level == aff_node->level); + rc = psci_get_aff_map_nodes(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + assert (rc == PSCI_E_SUCCESS); /* - * This function acquires the lock corresponding to each - * affinity level so that by the time we hit the highest - * affinity level, the system topology is snapshot and state - * management can be done safely. + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. */ - bakery_lock_get(mpidr, &aff_node->lock); - - /* Keep the old and new state handy */ - prev_state = psci_get_state(aff_node->state); - next_state = PSCI_STATE_ON; + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); /* Perform generic, architecture and platform specific handling */ - rc = pon_handlers[level](mpidr, aff_node, prev_state); - if (rc != PSCI_E_SUCCESS) { - psci_set_state(aff_node->state, prev_state); - goto exit; - } + rc = psci_call_power_on_handlers(mpidr_nodes, + start_afflvl, + end_afflvl, + pon_handlers, + mpidr); + assert (rc == PSCI_E_SUCCESS); /* - * State management: Update the states if this is the highest - * affinity level requested else pass the job to the next level. + * State management: Update the state of each affinity instance + * between the start and end affinity levels */ - if (aff_node->level != tgt_afflvl) { - rc = psci_afflvl_power_on_finish(mpidr, - level + 1, - tgt_afflvl, - pon_handlers); - } else { - psci_change_state(mpidr, MPIDR_AFFLVL0, tgt_afflvl, next_state); - } - - /* If all has gone as per plan then this cpu should be marked as ON */ - if (level == MPIDR_AFFLVL0) { - next_state = psci_get_state(aff_node->state); - assert(next_state == PSCI_STATE_ON); - } + psci_change_state(mpidr_nodes, + start_afflvl, + end_afflvl, + PSCI_STATE_ON); -exit: - bakery_lock_release(mpidr, &aff_node->lock); - return rc; + /* + * This loop releases the lock corresponding to each affinity level + * in the reverse order to which they were acquired. + */ + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); } diff --git a/common/psci/psci_main.c b/common/psci/psci_main.c index fbf864b..f73fc49 100644 --- a/common/psci/psci_main.c +++ b/common/psci/psci_main.c @@ -45,7 +45,7 @@ int psci_cpu_on(unsigned long target_cpu, { int rc; - unsigned int start_afflvl, target_afflvl; + unsigned int start_afflvl, end_afflvl; /* Determine if the cpu exists of not */ rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0); @@ -53,13 +53,17 @@ int psci_cpu_on(unsigned long target_cpu, goto exit; } - start_afflvl = get_max_afflvl(); - target_afflvl = MPIDR_AFFLVL0; + /* + * To turn this cpu on, specify which affinity + * levels need to be turned on + */ + start_afflvl = MPIDR_AFFLVL0; + end_afflvl = get_max_afflvl(); rc = psci_afflvl_on(target_cpu, entrypoint, context_id, start_afflvl, - target_afflvl); + end_afflvl); exit: return rc; @@ -76,7 +80,7 @@ int psci_cpu_suspend(unsigned int power_state, { int rc; unsigned long mpidr; - unsigned int tgt_afflvl, pstate_type; + unsigned int target_afflvl, pstate_type; /* TODO: Standby states are not supported at the moment */ pstate_type = psci_get_pstate_type(power_state); @@ -86,8 +90,8 @@ int psci_cpu_suspend(unsigned int power_state, } /* Sanity check the requested state */ - tgt_afflvl = psci_get_pstate_afflvl(power_state); - if (tgt_afflvl > MPIDR_MAX_AFFLVL) { + target_afflvl = psci_get_pstate_afflvl(power_state); + if (target_afflvl > MPIDR_MAX_AFFLVL) { rc = PSCI_E_INVALID_PARAMS; goto exit; } @@ -97,8 +101,8 @@ int psci_cpu_suspend(unsigned int power_state, entrypoint, context_id, power_state, - tgt_afflvl, - MPIDR_AFFLVL0); + MPIDR_AFFLVL0, + target_afflvl); exit: if (rc != PSCI_E_SUCCESS) @@ -120,7 +124,7 @@ int psci_cpu_off(void) * management is done immediately followed by cpu, cluster ... * ..target_afflvl specific actions as this function unwinds back. */ - rc = psci_afflvl_off(mpidr, target_afflvl, MPIDR_AFFLVL0); + rc = psci_afflvl_off(mpidr, MPIDR_AFFLVL0, target_afflvl); /* * The only error cpu_off can return is E_DENIED. So check if that's diff --git a/common/psci/psci_private.h b/common/psci/psci_private.h index e2100f8..7338f1c 100644 --- a/common/psci/psci_private.h +++ b/common/psci/psci_private.h @@ -84,9 +84,9 @@ typedef struct { int max; } aff_limits_node; +typedef aff_map_node *mpidr_aff_map_nodes[MPIDR_MAX_AFFLVL]; typedef unsigned int (*afflvl_power_on_finisher)(unsigned long, - aff_map_node *, - unsigned int); + aff_map_node *); /******************************************************************************* * Data prototypes @@ -110,9 +110,9 @@ extern unsigned int psci_get_aff_phys_state(aff_map_node *); extern unsigned int psci_calculate_affinity_state(aff_map_node *); extern void psci_get_ns_entry_info(unsigned int index); extern unsigned long mpidr_set_aff_inst(unsigned long,unsigned char, int); -extern int psci_change_state(unsigned long, int, int, unsigned int); +extern int psci_change_state(mpidr_aff_map_nodes, int, int, unsigned int); extern int psci_validate_mpidr(unsigned long, int); -extern unsigned int psci_afflvl_power_on_finish(unsigned long, +extern void psci_afflvl_power_on_finish(unsigned long, int, int, afflvl_power_on_finisher *); @@ -122,7 +122,21 @@ extern int psci_set_ns_entry_info(unsigned int index, extern int psci_get_first_present_afflvl(unsigned long, int, int, aff_map_node **); +extern int psci_check_afflvl_range(int start_afflvl, int end_afflvl); +extern void psci_acquire_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes mpidr_nodes); +extern void psci_release_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes mpidr_nodes); + /* Private exported functions from psci_setup.c */ +extern int psci_get_aff_map_nodes(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes mpidr_nodes); extern aff_map_node *psci_get_aff_map_node(unsigned long, int); /* Private exported functions from psci_affinity_on.c */ diff --git a/common/psci/psci_setup.c b/common/psci/psci_setup.c index 8220b30..decc18d 100644 --- a/common/psci/psci_setup.c +++ b/common/psci/psci_setup.c @@ -87,6 +87,56 @@ aff_map_node *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl) } /******************************************************************************* + * This function populates an array with nodes corresponding to a given range of + * affinity levels in an mpidr. It returns successfully only when the affinity + * levels are correct, the mpidr is valid i.e. no affinity level is absent from + * the topology tree & the affinity instance at level 0 is not absent. + ******************************************************************************/ +int psci_get_aff_map_nodes(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes mpidr_nodes) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node *node; + + rc = psci_check_afflvl_range(start_afflvl, end_afflvl); + if (rc != PSCI_E_SUCCESS) + return rc; + + for (level = start_afflvl; level <= end_afflvl; level++) { + + /* + * Grab the node for each affinity level. No affinity level + * can be missing as that would mean that the topology tree + * is corrupted. + */ + node = psci_get_aff_map_node(mpidr, level); + if (node == NULL) { + rc = PSCI_E_INVALID_PARAMS; + break; + } + + /* + * Skip absent affinity levels unless it's afffinity level 0. + * An absent cpu means that the mpidr is invalid. Save the + * pointer to the node for the present affinity level + */ + if (!(node->state & PSCI_AFF_PRESENT)) { + if (level == MPIDR_AFFLVL0) { + rc = PSCI_E_INVALID_PARAMS; + break; + } + + mpidr_nodes[level] = NULL; + } else + mpidr_nodes[level] = node; + } + + return rc; +} + +/******************************************************************************* * 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 |