ARM: imx: replicate the diagnostic register of boot cpu into secondary cores

The diagnostic register holds the errata bits.  Mostly bootloader
does not bring up secondary cores, so that when errata bits are set
in bootloader, they are set only for boot cpu.  But on a SMP
configuration, it should be equally done on every single core.
Set up the diagnostic register for secondary cores by replicating
the register from boot cpu.

Signed-off-by: Shawn Guo <shawn.guo@linaro.org>
Acked-by: Dirk Behme <dirk.behme@de.bosch.com>

1 files changed, 12 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/headsmp.S b/arch/arm/mach-imx/headsmp.S
index 67b9c48dcafe..627f16f0e9d1 100644
--- a/arch/arm/mach-imx/headsmp.S
+++ b/arch/arm/mach-imx/headsmp.S
@@ -18,8 +18,20 @@
         .section ".text.head", "ax"
 
 #ifdef CONFIG_SMP
+diag_reg_offset:
+        .word   g_diag_reg - .
+
+        .macro  set_diag_reg
+        adr     r0, diag_reg_offset
+        ldr     r1, [r0]
+        add     r1, r1, r0              @ r1 = physical &g_diag_reg
+        ldr     r0, [r1]
+        mcr     p15, 0, r0, c15, c0, 1  @ write diagnostic register
+        .endm
+
 ENTRY(v7_secondary_startup)
         bl      v7_invalidate_l1
+        set_diag_reg
         b       secondary_startup
 ENDPROC(v7_secondary_startup)
 #endif