utils: Add cpu_sleep() function

Signed-off-by: Hector Martin <[email protected]>

Author: marcan

proxyclient/m1n1/proxy.py

@@ -488,6 +488,7 @@ class M1N1Proxy(Reloadable):
     P_GET_SIMD_STATE = 0x00e
     P_PUT_SIMD_STATE = 0x00f
     P_REBOOT = 0x010
+    P_SLEEP = 0x011
 
     P_WRITE64 = 0x100
     P_WRITE32 = 0x101
@@ -744,6 +745,8 @@ def put_simd_state(self, buf):
         self.request(self.P_PUT_SIMD_STATE, buf)
     def reboot(self):
         self.request(self.P_REBOOT, no_reply=True)
+    def sleep(self, deep=False):
+        self.request(self.P_SLEEP, deep, no_reply=True)
 
     def write64(self, addr, data):
         '''write 8 byte value to given address'''

src/proxy.c

@@ -112,6 +112,9 @@ int proxy_process(ProxyRequest *request, ProxyReply *reply)
         case P_REBOOT:
             reboot();
             break;
+        case P_SLEEP:
+            cpu_sleep(request->args[0]);
+            break;
 
         case P_WRITE64:
             exc_guard = GUARD_SKIP;

src/proxy.h

@@ -23,6 +23,7 @@ typedef enum {
     P_GET_SIMD_STATE,
     P_PUT_SIMD_STATE,
     P_REBOOT,
+    P_SLEEP,
 
     P_WRITE64 = 0x100, // Generic register functions
     P_WRITE32,

src/utils.h

@@ -430,6 +430,7 @@ extern u32 board_id, chip_id;
 
 extern struct vector_args next_stage;
 
+void cpu_sleep(bool deep) __attribute__((noreturn));
 void deep_wfi(void);
 
 bool is_heap(void *addr);

src/utils_asm.S

@@ -178,3 +178,41 @@ deep_wfi:
     msr SYS_IMP_APL_CYC_OVRD, x0
 
     ret
+
+.globl cpu_sleep
+.type cpu_sleep, @function
+cpu_sleep:
+    cmp x0, #0
+    bne 1f
+
+    mrs x1, SYS_IMP_APL_CYC_OVRD
+    and x1, x1, #~CYC_OVRD_IRQ_MODE_MASK
+    orr x1, x1, #CYC_OVRD_IRQ_MODE(2)
+    and x1, x1, #~CYC_OVRD_FIQ_MODE_MASK
+    orr x1, x1, #CYC_OVRD_FIQ_MODE(2)
+    msr SYS_IMP_APL_CYC_OVRD, x1
+1:
+
+    cmp x0, #0
+    beq 2f
+
+    mrs x1, SYS_IMP_APL_ACC_OVRD
+    orr x1, x1, #ACC_OVRD_PWR_DN_SRM(3)
+    and x1, x1, #~ACC_OVRD_DIS_L2_FLUSH_ACC_SLEEP_MASK
+    orr x1, x1, #ACC_OVRD_DIS_L2_FLUSH_ACC_SLEEP(2)
+    orr x1, x1, #ACC_OVRD_TRAIN_DOWN_LINK(3)
+    orr x1, x1, #ACC_OVRD_POWER_DOWN_CPM(3)
+    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:
+
+    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:
+    isb
+    wfi
+    b 3b