diff --git a/exosphere/src/arm.h b/exosphere/src/arm.h index 160a3a762..e845bfcc8 100644 --- a/exosphere/src/arm.h +++ b/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 diff --git a/exosphere/src/arm.s b/exosphere/src/arm.s index 0d5f58540..b93a02e9b 100644 --- a/exosphere/src/arm.s +++ b/exosphere/src/arm.s @@ -227,7 +227,46 @@ invalidate_icache_inner_shareable: dsb ish 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 diff --git a/exosphere/src/cpu_context.c b/exosphere/src/cpu_context.c index 29ae4ed88..0e511c481 100644 --- a/exosphere/src/cpu_context.c +++ b/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) { diff --git a/exosphere/src/cpu_context.h b/exosphere/src/cpu_context.h index f1236524c..d9f8c2617 100644 --- a/exosphere/src/cpu_context.h +++ b/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 diff --git a/exosphere/src/interrupt.c b/exosphere/src/interrupt.c index 52f42250c..ff5af5ebe 100644 --- a/exosphere/src/interrupt.c +++ b/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)); diff --git a/exosphere/src/interrupt.h b/exosphere/src/interrupt.h index 1563798bb..9dbc0e61a 100644 --- a/exosphere/src/interrupt.h +++ b/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);