From 099973469b430fdbccc6ab82737c760aeccfbfce Mon Sep 17 00:00:00 2001
From: Soby Mathew <soby.mathew@arm.com>
Date: Tue, 18 Nov 2014 10:14:14 +0000
Subject: [PATCH] Invalidate the dcache after initializing cpu-ops

This patch fixes a crash due to corruption of cpu_ops
data structure. During the secondary CPU boot, after the
cpu_ops has been initialized in the per cpu-data, the
dcache lines need to invalidated so that the update in
memory can be seen later on when the dcaches are turned ON.
Also, after initializing the psci per cpu data, the dcache
lines are flushed so that they are written back to memory
and dirty dcache lines are avoided.

Fixes ARM-Software/tf-issues#271

Change-Id: Ia90f55e9882690ead61226eea5a5a9146d35f313
---
 include/bl31/cpu_data.h            |  4 ++++
 lib/cpus/aarch64/cpu_helpers.S     | 14 +++++++++++++-
 services/std_svc/psci/psci_setup.c |  3 ++-
 3 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/include/bl31/cpu_data.h b/include/bl31/cpu_data.h
index ba7ae0633..c886e2b4e 100644
--- a/include/bl31/cpu_data.h
+++ b/include/bl31/cpu_data.h
@@ -115,6 +115,10 @@ void init_cpu_data_ptr(void);
 #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 f053d44f3..5680bce6b 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -120,7 +120,19 @@ func init_cpu_ops
 	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 8e9d15dc0..e0bc83313 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -219,10 +219,11 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 				      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;