diff options
Diffstat (limited to 'common')
| -rw-r--r-- | common/psci/psci_afflvl_on.c | 2 | ||||
| -rw-r--r-- | common/psci/psci_afflvl_suspend.c | 2 | ||||
| -rw-r--r-- | common/psci/psci_common.c | 20 | ||||
| -rw-r--r-- | common/psci/psci_entry.S | 9 | ||||
| -rw-r--r-- | common/psci/psci_private.h | 2 | ||||
| -rw-r--r-- | common/runtime_svc.c | 1 |
6 files changed, 27 insertions, 9 deletions
diff --git a/common/psci/psci_afflvl_on.c b/common/psci/psci_afflvl_on.c index 7126b98d..c118cab6 100644 --- a/common/psci/psci_afflvl_on.c +++ b/common/psci/psci_afflvl_on.c @@ -337,7 +337,7 @@ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr, * for restoring the re-entry info */ index = cpu_node->data; - rc = psci_get_ns_entry_info(index); + psci_get_ns_entry_info(index); /* Clean caches before re-entering normal world */ dcsw_op_louis(DCCSW); diff --git a/common/psci/psci_afflvl_suspend.c b/common/psci/psci_afflvl_suspend.c index b68b4df7..05b0aad0 100644 --- a/common/psci/psci_afflvl_suspend.c +++ b/common/psci/psci_afflvl_suspend.c @@ -389,7 +389,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 it's way. */ - rc = psci_get_ns_entry_info(index); + psci_get_ns_entry_info(index); /* Clean caches before re-entering normal world */ dcsw_op_louis(DCCSW); diff --git a/common/psci/psci_common.c b/common/psci/psci_common.c index 59126baa..8eb02133 100644 --- a/common/psci/psci_common.c +++ b/common/psci/psci_common.c @@ -36,6 +36,7 @@ #include <platform.h> #include <psci.h> #include <psci_private.h> +#include <runtime_svc.h> /******************************************************************************* * Arrays that contains information needs to resume a cpu's execution when woken @@ -280,9 +281,10 @@ unsigned int psci_calculate_affinity_state(aff_map_node *aff_node) * resume a cpu's execution in the non-secure state after it has been physically * powered on i.e. turned ON or resumed from SUSPEND ******************************************************************************/ -unsigned int psci_get_ns_entry_info(unsigned int index) +void psci_get_ns_entry_info(unsigned int index) { unsigned long sctlr = 0, scr, el_status, id_aa64pfr0; + gp_regs *ns_gp_regs; scr = read_scr(); @@ -318,10 +320,22 @@ unsigned int psci_get_ns_entry_info(unsigned int index) /* Fulfill the cpu_on entry reqs. as per the psci spec */ write_scr(scr); - write_spsr(psci_ns_entry_info[index].eret_info.spsr); write_elr(psci_ns_entry_info[index].eret_info.entrypoint); - return psci_ns_entry_info[index].context_id; + /* + * Set the general purpose registers to ~0 upon entry into the + * non-secure world except for x0 which should contain the + * context id & spsr. This is done directly on the "would be" + * stack pointer. Prior to entry into the non-secure world, an + * offset equivalent to the size of the 'gp_regs' structure is + * added to the sp. This general purpose register context is + * retrieved then. + */ + ns_gp_regs = (gp_regs *) platform_get_stack(read_mpidr()); + ns_gp_regs--; + memset(ns_gp_regs, ~0, sizeof(*ns_gp_regs)); + ns_gp_regs->x0 = psci_ns_entry_info[index].context_id; + ns_gp_regs->spsr = psci_ns_entry_info[index].eret_info.spsr; } /******************************************************************************* diff --git a/common/psci/psci_entry.S b/common/psci/psci_entry.S index 2f39f360..74cdf956 100644 --- a/common/psci/psci_entry.S +++ b/common/psci/psci_entry.S @@ -32,6 +32,7 @@ #include <platform.h> #include <psci.h> #include <psci_private.h> +#include <runtime_svc.h> #include <asm_macros.S> .globl psci_aff_on_finish_entry @@ -77,7 +78,6 @@ psci_aff_common_finish_entry: mov x0, x19 mov x1, #MPIDR_AFFLVL0 blr x22 - mov x21, x0 /* -------------------------------------------- * Give ourselves a stack allocated in Normal @@ -88,10 +88,13 @@ psci_aff_common_finish_entry: bl platform_set_stack /* -------------------------------------------- - * Restore the context id. value + * Use the size of the general purpose register + * context to restore the register state + * stashed by earlier code * -------------------------------------------- */ - mov x0, x21 + sub sp, sp, #SIZEOF_GPREGS + exception_exit restore_regs /* -------------------------------------------- * Jump back to the non-secure world assuming diff --git a/common/psci/psci_private.h b/common/psci/psci_private.h index 4741397c..e2100f83 100644 --- a/common/psci/psci_private.h +++ b/common/psci/psci_private.h @@ -108,7 +108,7 @@ extern int get_max_afflvl(void); extern unsigned int psci_get_phys_state(unsigned int); extern unsigned int psci_get_aff_phys_state(aff_map_node *); extern unsigned int psci_calculate_affinity_state(aff_map_node *); -extern unsigned int psci_get_ns_entry_info(unsigned int index); +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_validate_mpidr(unsigned long, int); diff --git a/common/runtime_svc.c b/common/runtime_svc.c index 0aa4460f..431cfa3c 100644 --- a/common/runtime_svc.c +++ b/common/runtime_svc.c @@ -38,6 +38,7 @@ #include <semihosting.h> #include <bl_common.h> #include <psci.h> +#include <runtime_svc.h> /******************************************************************************* * Perform initialization of runtime services possibly across exception levels |