From da3519de14268c0a5ba60c2396d786490d41f88a Mon Sep 17 00:00:00 2001
From: Nick Chan <towinchenmi@gmail.com>
Date: Mon, 7 Oct 2024 19:14:54 +0800
Subject: [PATCH] smp: Add support for deep sleep on A7-A9X

Signed-off-by: Nick Chan <towinchenmi@gmail.com>
---
 src/chickens.c  |  7 +++++++
 src/smp.c       |  3 ++-
 src/utils.h     |  1 +
 src/utils_asm.S | 20 ++++++++++++++++----
 4 files changed, 26 insertions(+), 5 deletions(-)

diff --git a/src/chickens.c b/src/chickens.c
index 08677744..2f7bcd8a 100644
--- a/src/chickens.c
+++ b/src/chickens.c
@@ -70,6 +70,7 @@ void init_t6031_sawtooth(void);
 void init_t6031_everest(int rev);
 
 bool cpufeat_actlr_el2, cpufeat_fast_ipi, cpufeat_mmu_sprr;
+bool cpufeat_global_sleep;
 
 const char *init_cpu(void)
 {
@@ -241,6 +242,12 @@ const char *init_cpu(void)
     if (part >= MIDR_PART_T8015_MONSOON)
         cpufeat_fast_ipi = true;
 
+    if (part >= MIDR_PART_T8010_2_HURRICANE)
+        cpufeat_global_sleep = true;
+    else
+        /* Disable deep sleep */
+        reg_clr(SYS_IMP_APL_ACC_CFG, ACC_CFG_DEEP_SLEEP);
+
     /* Unmask external IRQs, set WFI mode to up (2) */
     reg_mask(SYS_IMP_APL_CYC_OVRD,
              CYC_OVRD_FIQ_MODE_MASK | CYC_OVRD_IRQ_MODE_MASK | CYC_OVRD_WFI_MODE_MASK,
diff --git a/src/smp.c b/src/smp.c
index 74ed2ecc..efbde3b6 100644
--- a/src/smp.c
+++ b/src/smp.c
@@ -179,8 +179,9 @@ static void smp_stop_cpu(int index, int die, int cluster, int core, u64 impl, u6
     // Request CPU stop
     write32(cpu_start_base + 0x0, 1 << (4 * cluster + core));
 
+    u64 dsleep = deep_sleep;
     // Put the CPU to sleep
-    smp_call1(index, cpu_sleep, deep_sleep);
+    smp_call2(index, cpu_sleep, dsleep, cpufeat_global_sleep);
 
     // If going into deep sleep, powering off the last core in a cluster kills our register
     // access, so just wait a bit.
diff --git a/src/utils.h b/src/utils.h
index 250de40b..256ed5aa 100644
--- a/src/utils.h
+++ b/src/utils.h
@@ -460,6 +460,7 @@ extern u32 board_id, chip_id;
 
 extern bool is_mac, has_dcp;
 extern bool cpufeat_actlr_el2, cpufeat_fast_ipi, cpufeat_mmu_sprr;
+extern bool cpufeat_global_sleep;
 
 extern struct vector_args next_stage;
 extern u64 boot_flags, mem_size_actual;
diff --git a/src/utils_asm.S b/src/utils_asm.S
index 0deee5a7..2b901d3a 100644
--- a/src/utils_asm.S
+++ b/src/utils_asm.S
@@ -185,6 +185,8 @@ cpu_sleep:
     cmp x0, #0
     bne 1f
 
+    mov x4, x1
+
     mrs x1, SYS_IMP_APL_CYC_OVRD
     and x1, x1, #~CYC_OVRD_IRQ_MODE_MASK
     orr x1, x1, #CYC_OVRD_IRQ_MODE(2)
@@ -192,8 +194,10 @@ cpu_sleep:
     orr x1, x1, #CYC_OVRD_FIQ_MODE(2)
     msr SYS_IMP_APL_CYC_OVRD, x1
 1:
-
     cmp x0, #0
+    beq 3f
+
+    cmp x4, #0
     beq 2f
 
     mrs x1, SYS_IMP_APL_ACC_OVRD
@@ -205,14 +209,22 @@ cpu_sleep:
     orr x1, x1, #ACC_OVRD_DISABLE_PIO_ON_WFI_CPU
     orr x1, x1, #ACC_OVRD_DEEP_SLEEP
     msr SYS_IMP_APL_ACC_OVRD, x1
-2:
 
+    cmp x4, #0
+    bne 3f
+
+2:
+    mrs x1, SYS_IMP_APL_ACC_CFG
+    orr x1, x1, #ACC_CFG_DEEP_SLEEP
+    msr SYS_IMP_APL_ACC_CFG, x1
+
+3:
     mrs x1, SYS_IMP_APL_CYC_OVRD
     orr x1, x1, #CYC_OVRD_WFI_MODE(3)
     orr x1, x1, #CYC_OVRD_DISABLE_WFI_RET
     msr SYS_IMP_APL_CYC_OVRD, x1
 
-3:
+4:
     isb
     wfi
-    b 3b
+    b 4b