Finish smcCpuOff

exosphere/src/arm.h

@@ -17,7 +17,7 @@ void invalidate_dcache_range(const void *start, const void *end);
 
 void invalidate_icache_inner_shareable(void);
 
-
+void finalize_powerdown(void);
 void call_with_stack_pointer(uintptr_t stack_pointer, void (*function)(void));
 
 #endif

exosphere/src/arm.s

@@ -228,6 +228,45 @@ invalidate_icache_inner_shareable:
     isb
     ret
     
+/* Final steps before power down. */
+.section    .text.finalize_powerdown, "ax", %progbits
+.type       finalize_powerdown, %function
+.global     finalize_powerdown
+finalize_powerdown:
+    /* All data access to Normal memory from EL0/EL1 + all Normal Memory accesses to EL0/1 stage 1 translation tables non-cacheable for all levels, unified cache. */
+    mrs x0, sctlr_el1
+    and x0, x0, #0xfffffffffffffffb
+    msr sctlr_el1, x0
+    isb
+    /* Same as above, for EL3. */
+    mrs x0, sctlr_el3
+    and x0, x0, #0xfffffffffffffffb
+    msr sctlr_el3, x0
+    isb
+    /* Disable table walk descriptor access prefetch, disable instruction prefetch, disable data prefetch. */
+    mrs x0, s3_1_c15_c2_1
+    orr x0, x0, #0x4000000000
+    and x0, x0, #0xffffffe7ffffffff
+    and x0, x0, #0xfffffffcffffffff
+    msr s3_1_c15_c2_1, x0
+    isb
+    dsb sy
+    bl flush_dcache_all
+    /* Disable receiving instruction cache/tbl maintenance operations. */
+    mrs x0, s3_1_c15_c2_1
+    and x0, x0, #0xffffffffffffffbf
+    msr s3_1_c15_c2_1, x0 
+    /* Prepare GICC */
+    bl intr_prepare_gicc_for_sleep
+    /* Set OS double lock */
+    mrs x0, osdlr_el1
+    orr x0, x0, #1
+    msr osdlr_el1, x0
+    isb
+    dsb sy
+wait_for_power_off:
+    wfi
+    b wait_for_power_off
     
 /* Call a function with desired stack pointer. */
 .section    .text.call_with_stack_pointer, "ax", %progbits

exosphere/src/cpu_context.c

@@ -60,7 +60,6 @@ uint32_t cpu_on(uint32_t core, uint64_t entrypoint_addr, uint64_t argument) {
     return 0;
 }
 
-
 void power_down_current_core(void) {
     unsigned int current_core = get_core_id();
     flow_set_csr(current_core, 0);
@@ -69,7 +68,7 @@ void power_down_current_core(void) {
     save_current_core_context();
     g_cpu_contexts[current_core].is_active = 0;
     flush_dcache_all();
-    /* TODO: wait_for_power_off(), which writes to regs + . */
+    finalize_powerdown();
 }
 
 uint32_t cpu_off(void) {
@@ -77,6 +76,7 @@ uint32_t cpu_off(void) {
     if (current_core == 3) {
         power_down_current_core();
     } else {
+        clear_priv_smc_in_progress();
         call_with_stack_pointer(get_exception_entry_stack_address(current_core), power_down_current_core);
     }
     while (true) {

exosphere/src/cpu_context.h

@@ -53,7 +53,7 @@ void set_current_core_inactive(void);
 void set_core_entrypoint_and_argument(uint32_t core, uint64_t entrypoint_addr, uint64_t argument);
 
 uint32_t cpu_on(uint32_t core, uint64_t entrypoint_addr, uint64_t argument);
-uint32_t cpu_off(void); /* TODO */
+uint32_t cpu_off(void);
 
 
 #endif

exosphere/src/interrupt.c

@@ -29,6 +29,11 @@ void intr_initialize_gic(void) {
     GICC_BPR = 7;
 }
 
+/* Sets GICC_CTLR to appropriate pre-sleep value. */
+void intr_prepare_gicc_for_sleep(void) {
+    GICC_CTLR = 0x1E0;
+}
+
 /* Sets an interrupt's group in the GICD. */
 void intr_set_group(unsigned int id, int group) {
     GICD_IGROUPR[id >> 5] = (GICD_IGROUPR[id >> 5] & (~(1 << (id & 0x1F)))) | ((group & 1) << (id & 0x1F));

exosphere/src/interrupt.h

@@ -47,6 +47,8 @@ void handle_registered_interrupt(void);
 /* Initializes the GIC. TODO: This must be called during wakeup. */
 void intr_initialize_gic(void);
 
+void intr_prepare_gicc_for_sleep(void);
+
 
 void intr_register_handler(unsigned int id, void (*handler)(void));
 void intr_set_group(unsigned int id, int group);