@@ -36,17 +36,7 @@ psci_cpu_on:
@ r1 = 0xf01
and r1, r1, #0xff
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_TARGET_PC_OFFSET
- str r2, [r0]
- dsb
-
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
- str r3, [r0]
- dsb
+ bl psci_cpu_on_common
@ Get DCFG base address
movw r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
@@ -29,17 +29,7 @@ psci_arch_init:
psci_cpu_on:
push {lr}
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_TARGET_PC_OFFSET
- str r2, [r0]
- dsb
-
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
- str r3, [r0]
- dsb
+ bl psci_cpu_on_common
ldr r2, =psci_cpu_entry
bl imx_cpu_on
@@ -253,6 +253,21 @@ ENTRY(psci_enable_smp)
ENDPROC(psci_enable_smp)
.weak psci_enable_smp
+/* expects target CPU in r1, target PC in r2, target conetxt ID in r3 */
+ENTRY(psci_cpu_on_common)
+ push {lr}
+
+ mov r0, r1
+ bl psci_get_cpu_stack_top @ get stack top of target CPU
+ sub r5, r0, #PSCI_TARGET_PC_OFFSET
+ str r2, [r5] @ save target PC
+ sub r5, r0, #PSCI_CONTEXT_ID_OFFSET
+ str r3, [r5] @ save target context ID
+ dsb
+
+ pop {pc}
+ENDPROC(psci_cpu_on_common)
+
ENTRY(psci_cpu_off_common)
push {lr}
@@ -135,17 +135,7 @@ out: mcr p15, 0, r7, c1, c1, 0
psci_cpu_on:
push {lr}
- mov r0, r1
- bl psci_get_cpu_stack_top @ get stack top of target CPU
- sub r0, r0, #PSCI_TARGET_PC_OFFSET
- str r2, [r0] @ store target PC
- dsb
-
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
- str r3, [r0]
- dsb
+ bl psci_cpu_on_common
movw r0, #(SUN6I_CPUCFG_BASE & 0xffff)
movt r0, #(SUN6I_CPUCFG_BASE >> 16)
@@ -124,18 +124,7 @@ out: mcr p15, 0, r7, c1, c1, 0
psci_cpu_on:
push {lr}
- mov r0, r1
- bl psci_get_cpu_stack_top @ get stack top of target CPU
- sub r0, r0, #PSCI_TARGET_PC_OFFSET
- str r2, [r0] @ store target PC
- dsb
-
-
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
- str r3, [r0]
- dsb
+ bl psci_cpu_on_common
movw r0, #(SUN7I_CPUCFG_BASE & 0xffff)
movt r0, #(SUN7I_CPUCFG_BASE >> 16)
@@ -90,17 +90,7 @@ ENDPROC(psci_cpu_off)
ENTRY(psci_cpu_on)
push {lr}
- mov r0, r1
- bl psci_get_cpu_stack_top @ get stack top of target CPU
- sub r0, r0, #PSCI_TARGET_PC_OFFSET
- str r2, [r0] @ store target PC
- dsb
-
- mov r0, r1
- bl psci_get_cpu_stack_top
- sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
- str r3, [r0]
- dsb
+ bl psci_cpu_on_common
ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR
ldr r5, =psci_cpu_entry