diff --git a/include/bl31/cpu_data.h b/include/bl31/cpu_data.h index ba7ae06..c886e2b 100644 --- a/include/bl31/cpu_data.h +++ b/include/bl31/cpu_data.h @@ -115,6 +115,10 @@ #define flush_cpu_data(_m) flush_dcache_range((uint64_t) \ &(_cpu_data()->_m), \ sizeof(_cpu_data()->_m)) +#define flush_cpu_data_by_index(_ix, _m) \ + flush_dcache_range((uint64_t) \ + &(_cpu_data_by_index(_ix)->_m), \ + sizeof(_cpu_data_by_index(_ix)->_m)) #endif /* __ASSEMBLY__ */ diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S index f053d44..5680bce 100644 --- a/lib/cpus/aarch64/cpu_helpers.S +++ b/lib/cpus/aarch64/cpu_helpers.S @@ -120,7 +120,19 @@ cmp x0, #0 ASM_ASSERT(ne) #endif - str x0, [x6, #CPU_DATA_CPU_OPS_PTR] + str x0, [x6, #CPU_DATA_CPU_OPS_PTR]! + + /* + * Make sure that any pre-fetched cache copies are invalidated. + * Ensure that we are running with cache disable else we + * invalidate our own update. + */ +#if ASM_ASSERTION + mrs x1, sctlr_el3 + tst x1, #SCTLR_C_BIT + ASM_ASSERT(eq) +#endif + dc ivac, x6 mov x30, x10 1: ret diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c index 8e9d15d..e0bc833 100644 --- a/services/std_svc/psci/psci_setup.c +++ b/services/std_svc/psci/psci_setup.c @@ -219,10 +219,11 @@ psci_svc_cpu_data.max_phys_off_afflvl, PSCI_INVALID_DATA); + flush_cpu_data_by_index(linear_id, psci_svc_cpu_data); + cm_set_context_by_mpidr(mpidr, (void *) &psci_ns_context[linear_id], NON_SECURE); - } return;