@@ -67,6 +67,22 @@ psci_cpu_on:
beq out_psci_cpu_on
mov r1, r0
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_CPU_STATUS_OFFSET
+ ldr r5, [r0]
+
+ cmp r5, #PSCI_CPU_STATUS_ON
+ moveq r0, #ARM_PSCI_RET_ALREADY_ON
+ beq out_psci_cpu_on
+
+ cmp r5, #PSCI_CPU_STATUS_ON_PENDING
+ moveq r0, #ARM_PSCI_RET_ON_PENDING
+ beq out_psci_cpu_on
+
+ mov r5, #PSCI_CPU_STATUS_ON_PENDING
+ str r5, [r0]
+ dsb
+
bl psci_cpu_on_common
@ Get DCFG base address
@@ -124,6 +140,12 @@ holdoff_release:
rev r6, r6
str r6, [r4, #DCFG_CCSR_SCRATCHRW1]
+ mov r0, r1
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_CPU_STATUS_OFFSET
+ mov r5, #PSCI_CPU_STATUS_ON
+ str r5, [r0]
+
isb
dsb
@@ -138,6 +160,13 @@ out_psci_cpu_on:
psci_cpu_off:
bl psci_cpu_off_common
+ bl psci_get_cpu_id
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_CPU_STATUS_OFFSET
+ mov r5, #PSCI_CPU_STATUS_OFF
+ str r5, [r0]
+ dsb
+
1: wfi
b 1b
@@ -67,6 +67,11 @@
#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)
+#define PSCI_CPU_STATUS_OFFSET (PSCI_PERCPU_STACK_SIZE - 12)
+
+#define PSCI_CPU_STATUS_OFF 0
+#define PSCI_CPU_STATUS_ON 1
+#define PSCI_CPU_STATUS_ON_PENDING 2
#ifndef __ASSEMBLY__
int psci_update_dt(void *fdt);