diff options
Diffstat (limited to 'services/std_svc/psci/psci_afflvl_suspend.c')
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_suspend.c | 21 |
1 files changed, 11 insertions, 10 deletions
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c index 99343104..f43dcedf 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -132,10 +132,12 @@ static int psci_afflvl0_suspend(unsigned long mpidr, unsigned long context_id, unsigned int power_state) { - unsigned int index, plat_state; + unsigned int plat_state; unsigned long psci_entrypoint, sctlr; el3_state_t *saved_el3_state; - int rc = PSCI_E_SUCCESS; + uint32_t ns_scr_el3 = read_scr_el3(); + uint32_t ns_sctlr_el1 = read_sctlr_el1(); + int rc; /* Sanity check to safeguard against data corruption */ assert(cpu_node->level == MPIDR_AFFLVL0); @@ -163,8 +165,8 @@ static int psci_afflvl0_suspend(unsigned long mpidr, * Generic management: Store the re-entry information for the * non-secure world */ - index = cpu_node->data; - rc = psci_set_ns_entry_info(index, ns_entrypoint, context_id); + rc = psci_save_ns_entry(read_mpidr_el1(), ns_entrypoint, context_id, + ns_scr_el3, ns_sctlr_el1); if (rc != PSCI_E_SUCCESS) return rc; @@ -174,7 +176,6 @@ static int psci_afflvl0_suspend(unsigned long mpidr, * L1 caches and exit intra-cluster coherency et al */ cm_el3_sysregs_context_save(NON_SECURE); - rc = PSCI_E_SUCCESS; /* * The EL3 state to PoC since it will be accessed after a @@ -214,6 +215,8 @@ static int psci_afflvl0_suspend(unsigned long mpidr, * platform defined mailbox with the psci entrypoint, * program the power controller etc. */ + rc = PSCI_E_SUCCESS; + if (psci_plat_pm_ops->affinst_suspend) { plat_state = psci_get_phys_state(cpu_node); rc = psci_plat_pm_ops->affinst_suspend(mpidr, @@ -454,7 +457,7 @@ int psci_afflvl_suspend(unsigned long mpidr, static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, aff_map_node_t *cpu_node) { - unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS; + unsigned int plat_state, state, rc; int32_t suspend_level; assert(cpu_node->level == MPIDR_AFFLVL0); @@ -481,14 +484,11 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, } /* Get the index for restoring the re-entry information */ - index = cpu_node->data; - /* * Arch. management: Restore the stashed EL3 architectural * context from the 'cpu_context' structure for this cpu. */ cm_el3_sysregs_context_restore(NON_SECURE); - rc = PSCI_E_SUCCESS; /* * Call the cpu suspend finish handler registered by the Secure Payload @@ -509,7 +509,7 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, * information that we had stashed away during the suspend * call to set this cpu on its way. */ - psci_get_ns_entry_info(index); + cm_prepare_el3_exit(NON_SECURE); /* State management: mark this cpu as on */ psci_set_state(cpu_node, PSCI_STATE_ON); @@ -517,6 +517,7 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, /* Clean caches before re-entering normal world */ dcsw_op_louis(DCCSW); + rc = PSCI_E_SUCCESS; return rc; } |