arm: mx6: use fuse_read() for OCOTP fuse access

[karo-tx-uboot.git] / arch / arm / cpu / armv7 / psci.S

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index cdcdccdc47f16d55be9ccaedda675b73c697df4c..87c0c0b6f5ebadabb312ee85c0689b1a5cd4e3ed 100644 (file)
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -17,6 +17,7 @@
 
 #include <config.h>
 #include <linux/linkage.h>
+#include <asm/macro.h>
 #include <asm/psci.h>
 
        .pushsection ._secure.text, "ax"
@@ -165,6 +166,15 @@ ENTRY(psci_disable_smp)
 ENDPROC(psci_disable_smp)
 .weak psci_disable_smp
 
+ENTRY(psci_enable_smp)
+       mrc     p15, 0, r0, c1, c0, 1           @ ACTLR
+       orr     r0, r0, #(1 << 6)               @ Set SMP bit
+       mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
+       isb
+       bx      lr
+ENDPROC(psci_enable_smp)
+.weak psci_enable_smp
+
 ENTRY(psci_cpu_off_common)
        push    {lr}
 
@@ -184,4 +194,30 @@ ENTRY(psci_cpu_off_common)
        bx      lr
 ENDPROC(psci_cpu_off_common)
 
+@ expects CPU ID in r0 and returns stack top in r0
+ENTRY(psci_get_cpu_stack_top)
+       mov     r5, #0x400                      @ 1kB of stack per CPU
+       mul     r0, r0, r5
+
+       ldr     r5, =psci_text_end              @ end of monitor text
+       add     r5, r5, #0x2000                 @ Skip two pages
+       lsr     r5, r5, #12                     @ Align to start of page
+       lsl     r5, r5, #12
+       sub     r5, r5, #4                      @ reserve 1 word for target PC
+       sub     r0, r5, r0                      @ here's our stack!
+
+       bx      lr
+ENDPROC(psci_get_cpu_stack_top)
+
+ENTRY(psci_cpu_entry)
+       bl      psci_enable_smp
+
+       bl      _nonsec_init
+
+       bl      psci_get_cpu_id                 @ CPU ID => r0
+       bl      psci_get_cpu_stack_top          @ stack top => r0
+       ldr     r0, [r0]                        @ target PC at stack top
+       b       _do_nonsec_entry
+ENDPROC(psci_cpu_entry)
+
        .popsection