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

@@ -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__ */

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

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;