ARM: Factor out common psci_get_cpu_id

Will be required for obtaining the ID of the current CPU in shared PSCI
functions. The default implementation requires a dense ID space and only
supports a single cluster. Therefore, the functions can be overloaded in
cases where these assumptions do not hold.

CC: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Jan Kiszka <jan.kiszka@siemens.com>
Reviewed-by: Tom Rini <trini@konsulko.com>
Reviewed-by: Thierry Reding <treding@nvidia.com>
Tested-by: Thierry Reding <treding@nvidia.com>
Tested-by: Ian Campbell <ijc@hellion.org.uk>
Acked-by: Ian Campbell <ijc@hellion.org.uk>
Signed-off-by: Tom Warren <twarren@nvidia.com>

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index bf11a34..12ad09b 100644 (file)
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -99,4 +99,12 @@ _smc_psci:
        pop     {r4-r7, lr}
        movs    pc, lr                  @ Return to the kernel
 
+@ Requires dense and single-cluster CPU ID space
+ENTRY(psci_get_cpu_id)
+       mrc     p15, 0, r0, c0, c0, 5   /* read MPIDR */
+       and     r0, r0, #0xff           /* return CPU ID in cluster */
+       bx      lr
+ENDPROC(psci_get_cpu_id)
+.weak psci_get_cpu_id
+
        .popsection

diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S
index 07b2d76..bcc419d 100644 (file)
--- a/arch/arm/cpu/armv7/sunxi/psci.S
+++ b/arch/arm/cpu/armv7/sunxi/psci.S
@@ -19,6 +19,7 @@
 
 #include <config.h>
 #include <asm/gic.h>
+#include <asm/macro.h>
 #include <asm/psci.h>
 #include <asm/arch/cpu.h>
 
@@ -290,6 +291,8 @@ psci_cpu_off:
 
 .globl psci_arch_init
 psci_arch_init:
+       mov     r6, lr
+
        movw    r4, #(GICD_BASE & 0xffff)
        movt    r4, #(GICD_BASE >> 16)
 
@@ -315,18 +318,17 @@ psci_arch_init:
        mcr     p15, 0, r5, c1, c1, 0   @ Write SCR
        isb
 
-       mrc     p15, 0, r4, c0, c0, 5   @ MPIDR
-       and     r4, r4, #3              @ cpu number in cluster
+       bl      psci_get_cpu_id
        mov     r5, #0x400              @ 1kB of stack per CPU
-       mul     r4, r4, r5
+       mul     r0, r0, r5
 
        adr     r5, text_end            @ end of text
        add     r5, r5, #0x2000         @ Skip two pages
        lsr     r5, r5, #12             @ Align to start of page
        lsl     r5, r5, #12
-       sub     sp, r5, r4              @ here's our stack!
+       sub     sp, r5, r0              @ here's our stack!
 
-       bx      lr
+       bx      r6
 
 text_end:
        .popsection