/*******************************************************************************
* Routine to return the maximum affinity level to traverse to after a cpu has
* been physically powered up. It is expected to be called immediately after
* reset from assembler code.
******************************************************************************/
int get_power_on_target_afflvl(void)
{
int afflvl;
#if DEBUG
unsigned int state;
aff_map_node_t *node;
/* Retrieve our node from the topology tree */
node = psci_get_aff_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);
assert(state == PSCI_STATE_SUSPEND || state == PSCI_STATE_ON_PENDING);
#endif
/*
* Assume that this cpu was suspended and retrieve its target affinity
* level. If it is invalid then it could only have been turned off
* earlier. PLATFORM_MAX_AFFLVL will be the highest affinity level a
* cpu can be turned off to.
*/
afflvl = psci_get_suspend_afflvl();
if (afflvl == PSCI_INVALID_DATA)
afflvl = PLATFORM_MAX_AFFLVL;
return afflvl;
}
/*******************************************************************************
* The following functions finish an earlier affinity suspend request. They
* are called by the common finisher routine in psci_common.c.
******************************************************************************/
static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
{
unsigned int plat_state, state, rc;
int32_t suspend_level;
uint64_t counter_freq;
assert(cpu_node->level == MPIDR_AFFLVL0);
/* Ensure we have been woken up from a suspended state */
state = psci_get_state(cpu_node);
assert(state == PSCI_STATE_SUSPEND);
/*
* Plat. management: Perform the platform specific actions
* before we change the state of the cpu e.g. enabling the
* gic or zeroing the mailbox register. If anything goes
* wrong then assert as there is no way to recover from this
* situation.
*/
if (psci_plat_pm_ops->affinst_suspend_finish) {
/* Get the physical state of this cpu */
plat_state = get_phys_state(state);
rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(),
cpu_node->level,
plat_state);
assert(rc == PSCI_E_SUCCESS);
}
/* Get the index for restoring the re-entry information */
/*
* Arch. management: Enable the data cache, manage stack memory and
* restore the stashed EL3 architectural context from the 'cpu_context'
* structure for this cpu.
*/
psci_do_pwrup_cache_maintenance();
/* Re-init the cntfrq_el0 register */
counter_freq = plat_get_syscnt_freq();
write_cntfrq_el0(counter_freq);
/*
* Call the cpu suspend 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
*/
if (psci_spd_pm && psci_spd_pm->svc_suspend) {
suspend_level = psci_get_suspend_afflvl();
assert (suspend_level != PSCI_INVALID_DATA);
psci_spd_pm->svc_suspend_finish(suspend_level);
}
/* Invalidate the suspend context for the node */
psci_set_suspend_power_state(PSCI_INVALID_DATA);
/*
* Generic management: Now we just need to retrieve the
* information that we had stashed away during the suspend
* call to set this cpu on its way.
*/
cm_prepare_el3_exit(NON_SECURE);
/* Clean caches before re-entering normal world */
dcsw_op_louis(DCCSW);
rc = PSCI_E_SUCCESS;
return rc;
}