@@ -27,6 +27,7 @@
@ r1 = target CPU
@ r2 = target PC
+ @ r3 = target Conetxt ID
.globl psci_cpu_on
psci_cpu_on:
push {lr}
@@ -41,6 +42,12 @@ psci_cpu_on:
str r2, [r0]
dsb
+ mov r0, r1
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
+ str r3, [r0]
+ dsb
+
@ Get DCFG base address
movw r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
movt r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
@@ -24,7 +24,7 @@ psci_arch_init:
@ r1 = target CPU
@ r2 = target PC
-
+ @ r3 = target Conetxt ID
.globl psci_cpu_on
psci_cpu_on:
push {lr}
@@ -35,6 +35,12 @@ psci_cpu_on:
str r2, [r0]
dsb
+ mov r0, r1
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
+ str r3, [r0]
+ dsb
+
ldr r2, =psci_cpu_entry
bl imx_cpu_on
@@ -11,6 +11,7 @@
#include <asm/gic.h>
#include <asm/armv7.h>
#include <asm/proc-armv/ptrace.h>
+#include <asm/psci.h>
.arch_extension sec
.arch_extension virt
@@ -89,6 +90,12 @@ _secure_monitor:
movne r4, #0
mcrrne p15, 4, r4, r4, c14 @ Reset CNTVOFF to zero
1:
+#ifdef CONFIG_ARMV7_PSCI
+ bl psci_get_cpu_id
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_CONTEXT_ID_OFFSET
+ ldr r0, [r0] @ get Context ID in r0
+#endif
mov lr, ip
mov ip, #(F_BIT | I_BIT | A_BIT) @ Set A, I and F
tst lr, #1 @ Check for Thumb PC
@@ -130,6 +130,7 @@ out: mcr p15, 0, r7, c1, c1, 0
@ r1 = target CPU
@ r2 = target PC
+ @ r3 = target Conetxt ID
.globl psci_cpu_on
psci_cpu_on:
push {lr}
@@ -140,6 +141,12 @@ psci_cpu_on:
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
+
movw r0, #(SUN6I_CPUCFG_BASE & 0xffff)
movt r0, #(SUN6I_CPUCFG_BASE >> 16)
@@ -119,6 +119,7 @@ out: mcr p15, 0, r7, c1, c1, 0
@ r1 = target CPU
@ r2 = target PC
+ @ r3 = target Conetxt ID
.globl psci_cpu_on
psci_cpu_on:
push {lr}
@@ -129,6 +130,13 @@ psci_cpu_on:
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
+
movw r0, #(SUN7I_CPUCFG_BASE & 0xffff)
movt r0, #(SUN7I_CPUCFG_BASE >> 16)
@@ -21,6 +21,7 @@
/* size of percpu stack, 1kB */
#define PSCI_PERCPU_STACK_SIZE 0x400
#define PSCI_TARGET_PC_OFFSET (PSCI_PERCPU_STACK_SIZE - 4)
+#define PSCI_CONTEXT_ID_OFFSET (PSCI_PERCPU_STACK_SIZE - 8)
/* PSCI interfaces */
#define PSCI_FN_BASE 0x84000000
@@ -96,6 +96,12 @@ ENTRY(psci_cpu_on)
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
+
ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR
ldr r5, =psci_cpu_entry
str r5, [r6]