diff --git a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu_system_registers.hpp b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu_system_registers.hpp index 7912bea3f..6b0fb93f5 100644 --- a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu_system_registers.hpp +++ b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu_system_registers.hpp @@ -56,26 +56,69 @@ namespace ams::kern::arch::arm64::cpu { MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(OslarEl1, oslar_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(TpidrEl0, tpidr_el0) MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(TpidrRoEl0, tpidrro_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(EsrEl1, esr_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(ElrEl1, elr_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(EsrEl1, esr_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(SpsrEl1, spsr_el1) MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(Afsr0El1, afsr0_el1) MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(Afsr1El1, afsr1_el1) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmUserEnrEl0, pmuserenr_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmcCntrEl0, pmccntr_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr0El0, pmevcntr0_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr1El0, pmevcntr1_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr2El0, pmevcntr2_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr3El0, pmevcntr3_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr4El0, pmevcntr4_el0) - MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr5El0, pmevcntr5_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(MdscrEl1, mdscr_el1) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(CpacrEl1, cpacr_el1) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(ContextidrEl1, contextidr_el1) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(CntkCtlEl1, cntkctl_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(CntpCtlEl0, cntp_ctl_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(CntpCvalEl0, cntp_cval_el0) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(Daif, daif) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(SpEl0, sp_el0) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(IdAa64Dfr0El1, id_aa64dfr0_el1) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmcrEl0, pmcr_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmUserEnrEl0, pmuserenr_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmcCntrEl0, pmccntr_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmSelrEl0, pmselr_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmcCfiltrEl0, pmccfiltr_el0) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmIntEnSetEl1, pmintenset_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmCntEnSetEl0, pmcntenset_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmOvsSetEl0, pmovsset_el0) + + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmIntEnClrEl1, pmintenclr_el1) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmCntEnClrEl0, pmcntenclr_el0) + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmOvsClrEl0, pmovsclr_el0) + + #define FOR_I_IN_0_TO_30(HANDLER, ...) \ + HANDLER(0, ## __VA_ARGS__) HANDLER(1, ## __VA_ARGS__) HANDLER(2, ## __VA_ARGS__) HANDLER(3, ## __VA_ARGS__) \ + HANDLER(4, ## __VA_ARGS__) HANDLER(5, ## __VA_ARGS__) HANDLER(6, ## __VA_ARGS__) HANDLER(7, ## __VA_ARGS__) \ + HANDLER(8, ## __VA_ARGS__) HANDLER(9, ## __VA_ARGS__) HANDLER(10, ## __VA_ARGS__) HANDLER(11, ## __VA_ARGS__) \ + HANDLER(12, ## __VA_ARGS__) HANDLER(13, ## __VA_ARGS__) HANDLER(14, ## __VA_ARGS__) HANDLER(15, ## __VA_ARGS__) \ + HANDLER(16, ## __VA_ARGS__) HANDLER(17, ## __VA_ARGS__) HANDLER(18, ## __VA_ARGS__) HANDLER(19, ## __VA_ARGS__) \ + HANDLER(20, ## __VA_ARGS__) HANDLER(21, ## __VA_ARGS__) HANDLER(22, ## __VA_ARGS__) HANDLER(23, ## __VA_ARGS__) \ + HANDLER(24, ## __VA_ARGS__) HANDLER(25, ## __VA_ARGS__) HANDLER(26, ## __VA_ARGS__) HANDLER(27, ## __VA_ARGS__) \ + HANDLER(28, ## __VA_ARGS__) HANDLER(29, ## __VA_ARGS__) HANDLER(30, ## __VA_ARGS__) + + #define MESOSPHERE_CPU_DEFINE_PMEV_ACCESSORS(ID, ...) \ + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevCntr##ID##El0, pmevcntr##ID##_el0) \ + MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(PmevTyper##ID##El0, pmevtyper##ID##_el0) + + FOR_I_IN_0_TO_30(MESOSPHERE_CPU_DEFINE_PMEV_ACCESSORS) + + #undef MESOSPHERE_CPU_DEFINE_PMEV_ACCESSORS + #undef FOR_I_IN_0_TO_30 #define FOR_I_IN_0_TO_15(HANDLER, ...) \ HANDLER(0, ## __VA_ARGS__) HANDLER(1, ## __VA_ARGS__) HANDLER(2, ## __VA_ARGS__) HANDLER(3, ## __VA_ARGS__) \ HANDLER(4, ## __VA_ARGS__) HANDLER(5, ## __VA_ARGS__) HANDLER(6, ## __VA_ARGS__) HANDLER(7, ## __VA_ARGS__) \ HANDLER(8, ## __VA_ARGS__) HANDLER(9, ## __VA_ARGS__) HANDLER(10, ## __VA_ARGS__) HANDLER(11, ## __VA_ARGS__) \ - HANDLER(12, ## __VA_ARGS__) HANDLER(13, ## __VA_ARGS__) HANDLER(14, ## __VA_ARGS__) HANDLER(15, ## __VA_ARGS__) \ + HANDLER(12, ## __VA_ARGS__) HANDLER(13, ## __VA_ARGS__) HANDLER(14, ## __VA_ARGS__) HANDLER(15, ## __VA_ARGS__) #define MESOSPHERE_CPU_DEFINE_DBG_SYSREG_ACCESSORS(ID, ...) \ MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(DbgWcr##ID##El1, dbgwcr##ID##_el1) \ @@ -158,6 +201,15 @@ namespace ams::kern::arch::arm64::cpu { const size_t shift_value = this->GetBits(16, 6); return size_t(1) << (size_t(64) - shift_value); } + + constexpr ALWAYS_INLINE bool GetEpd0() const { + return this->GetBits(7, 1) != 0; + } + + constexpr ALWAYS_INLINE decltype(auto) SetEpd0(bool set) { + this->SetBit(7, set); + return *this; + } }; MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(ArchitecturalFeatureAccessControl) { @@ -387,6 +439,27 @@ namespace ams::kern::arch::arm64::cpu { /* TODO: Other bitfield accessors? */ }; + MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(PerformanceMonitorsControl) { + public: + MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(PerformanceMonitorsControl, pmcr_el0) + public: + constexpr ALWAYS_INLINE u64 GetN() const { + return this->GetBits(11, 5); + } + + constexpr ALWAYS_INLINE decltype(auto) SetEventCounterReset(bool en) { + this->SetBit(1, en); + return *this; + } + + constexpr ALWAYS_INLINE decltype(auto) SetCycleCounterReset(bool en) { + this->SetBit(2, en); + return *this; + } + + /* TODO: Other bitfield accessors? */ + }; + #undef FOR_I_IN_0_TO_15 #undef MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS #undef MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS diff --git a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_controller.hpp b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_controller.hpp index cf1761a5d..68f63332b 100644 --- a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_controller.hpp +++ b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_controller.hpp @@ -112,17 +112,17 @@ namespace ams::kern::arch::arm64 { static constexpr s32 NumPriorityLevels = 4; public: struct LocalState { - u32 local_isenabler[NumLocalInterrupts / 32]; - u32 local_ipriorityr[NumLocalInterrupts / 4]; - u32 local_targetsr[NumLocalInterrupts / 4]; - u32 local_icfgr[NumLocalInterrupts / 16]; + u32 isenabler[NumLocalInterrupts / 32]; + u32 ipriorityr[NumLocalInterrupts / 4]; + u32 itargetsr[NumLocalInterrupts / 4]; + u32 icfgr[NumLocalInterrupts / 16]; }; struct GlobalState { - u32 global_isenabler[NumGlobalInterrupts / 32]; - u32 global_ipriorityr[NumGlobalInterrupts / 4]; - u32 global_targetsr[NumGlobalInterrupts / 4]; - u32 global_icfgr[NumGlobalInterrupts / 16]; + u32 isenabler[NumGlobalInterrupts / 32]; + u32 ipriorityr[NumGlobalInterrupts / 4]; + u32 itargetsr[NumGlobalInterrupts / 4]; + u32 icfgr[NumGlobalInterrupts / 16]; }; enum PriorityLevel : u8 { @@ -142,6 +142,11 @@ namespace ams::kern::arch::arm64 { void Initialize(s32 core_id); void Finalize(s32 core_id); + + void SaveCoreLocal(LocalState *state) const; + void SaveGlobal(GlobalState *state) const; + void RestoreCoreLocal(const LocalState *state) const; + void RestoreGlobal(const GlobalState *state) const; public: u32 GetIrq() const { return this->gicc->iar; diff --git a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_manager.hpp b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_manager.hpp index 5fbccd4b8..e84b7a228 100644 --- a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_manager.hpp +++ b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_interrupt_manager.hpp @@ -67,6 +67,9 @@ namespace ams::kern::arch::arm64 { NOINLINE void Initialize(s32 core_id); NOINLINE void Finalize(s32 core_id); + NOINLINE void Save(s32 core_id); + NOINLINE void Restore(s32 core_id); + bool IsInterruptDefined(s32 irq) const { return this->interrupt_controller.IsInterruptDefined(irq); } diff --git a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_supervisor_page_table.hpp b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_supervisor_page_table.hpp index 0a1f71872..e653d5be8 100644 --- a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_supervisor_page_table.hpp +++ b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_supervisor_page_table.hpp @@ -23,9 +23,9 @@ namespace ams::kern::arch::arm64 { class KSupervisorPageTable { private: KPageTable page_table; - u64 ttbr0[cpu::NumCores]; + u64 ttbr0_identity[cpu::NumCores]; public: - constexpr KSupervisorPageTable() : page_table(), ttbr0() { /* ... */ } + constexpr KSupervisorPageTable() : page_table(), ttbr0_identity() { /* ... */ } NOINLINE void Initialize(s32 core_id); @@ -62,6 +62,8 @@ namespace ams::kern::arch::arm64 { bool GetPhysicalAddress(KPhysicalAddress *out, KProcessAddress address) const { return this->page_table.GetPhysicalAddress(out, address); } + + constexpr u64 GetIdentityMapTtbr0(s32 core_id) const { return this->ttbr0_identity[core_id]; } }; } diff --git a/libraries/libmesosphere/include/mesosphere/board/nintendo/nx/kern_k_device_page_table.hpp b/libraries/libmesosphere/include/mesosphere/board/nintendo/nx/kern_k_device_page_table.hpp index 8f5025a9a..98b110944 100644 --- a/libraries/libmesosphere/include/mesosphere/board/nintendo/nx/kern_k_device_page_table.hpp +++ b/libraries/libmesosphere/include/mesosphere/board/nintendo/nx/kern_k_device_page_table.hpp @@ -80,6 +80,11 @@ namespace ams::kern::board::nintendo::nx { bool Compare(const KPageGroup &pg, KDeviceVirtualAddress device_address) const; public: static void Initialize(); + + static void Lock(); + static void Unlock(); + static void Sleep(); + static void Wakeup(); }; } \ No newline at end of file diff --git a/libraries/libmesosphere/include/mesosphere/kern_debug_log.hpp b/libraries/libmesosphere/include/mesosphere/kern_debug_log.hpp index 0e7f319b7..dfabbaa05 100644 --- a/libraries/libmesosphere/include/mesosphere/kern_debug_log.hpp +++ b/libraries/libmesosphere/include/mesosphere/kern_debug_log.hpp @@ -29,6 +29,10 @@ namespace ams::kern { static NOINLINE void VPrintf(const char *format, ::std::va_list vl); static NOINLINE Result PrintUserString(ams::kern::svc::KUserPointer user_str, size_t len); + + /* Functionality for preserving across sleep. */ + static NOINLINE void Save(); + static NOINLINE void Restore(); }; } diff --git a/libraries/libmesosphere/source/arch/arm64/kern_exception_handlers.cpp b/libraries/libmesosphere/source/arch/arm64/kern_exception_handlers.cpp index 383b6029e..89cd0b9f7 100644 --- a/libraries/libmesosphere/source/arch/arm64/kern_exception_handlers.cpp +++ b/libraries/libmesosphere/source/arch/arm64/kern_exception_handlers.cpp @@ -163,14 +163,18 @@ namespace ams::kern::arch::arm64 { HandleUserException(context, esr, far, afsr0, afsr1, data); } } else { - MESOSPHERE_LOG("Unhandled Exception in Supervisor Mode\n"); - MESOSPHERE_LOG("Current Process = %s\n", GetCurrentProcess().GetName()); + const s32 core_id = GetCurrentCoreId(); + + MESOSPHERE_LOG("%d: Unhandled Exception in Supervisor Mode\n", core_id); + if (GetCurrentProcessPointer() != nullptr) { + MESOSPHERE_LOG("%d: Current Process = %s\n", core_id, GetCurrentProcess().GetName()); + } for (size_t i = 0; i < 31; i++) { - MESOSPHERE_LOG("X[%02zu] = %016lx\n", i, context->x[i]); + MESOSPHERE_LOG("%d: X[%02zu] = %016lx\n", core_id, i, context->x[i]); } - MESOSPHERE_LOG("PC = %016lx\n", context->pc); - MESOSPHERE_LOG("SP = %016lx\n", context->sp); + MESOSPHERE_LOG("%d: PC = %016lx\n", core_id, context->pc); + MESOSPHERE_LOG("%d: SP = %016lx\n", core_id, context->sp); MESOSPHERE_PANIC("Unhandled Exception in Supervisor Mode\n"); } diff --git a/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_controller.cpp b/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_controller.cpp index 5cce0c31f..33fef41f7 100644 --- a/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_controller.cpp +++ b/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_controller.cpp @@ -84,4 +84,114 @@ namespace ams::kern::arch::arm64 { this->gicc = nullptr; } + void KInterruptController::SaveCoreLocal(LocalState *state) const { + /* Save isenabler. */ + for (size_t i = 0; i < util::size(state->isenabler); ++i) { + constexpr size_t Offset = 0; + state->isenabler[i] = this->gicd->isenabler[i + Offset]; + this->gicd->isenabler[i + Offset] = 0xFFFFFFFF; + } + + /* Save ipriorityr. */ + for (size_t i = 0; i < util::size(state->ipriorityr); ++i) { + constexpr size_t Offset = 0; + state->ipriorityr[i] = this->gicd->ipriorityr.words[i + Offset]; + this->gicd->ipriorityr.words[i + Offset] = 0xFFFFFFFF; + } + + /* Save itargetsr. */ + for (size_t i = 0; i < util::size(state->itargetsr); ++i) { + constexpr size_t Offset = 0; + state->itargetsr[i] = this->gicd->itargetsr.words[i + Offset]; + } + + /* Save icfgr. */ + for (size_t i = 0; i < util::size(state->icfgr); ++i) { + constexpr size_t Offset = 0; + state->icfgr[i] = this->gicd->icfgr[i + Offset]; + } + } + + void KInterruptController::SaveGlobal(GlobalState *state) const { + /* Save isenabler. */ + for (size_t i = 0; i < util::size(state->isenabler); ++i) { + constexpr size_t Offset = util::size(LocalState{}.isenabler); + state->isenabler[i] = this->gicd->isenabler[i + Offset]; + this->gicd->isenabler[i + Offset] = 0xFFFFFFFF; + } + + /* Save ipriorityr. */ + for (size_t i = 0; i < util::size(state->ipriorityr); ++i) { + constexpr size_t Offset = util::size(LocalState{}.ipriorityr); + state->ipriorityr[i] = this->gicd->ipriorityr.words[i + Offset]; + this->gicd->ipriorityr.words[i + Offset] = 0xFFFFFFFF; + } + + /* Save itargetsr. */ + for (size_t i = 0; i < util::size(state->itargetsr); ++i) { + constexpr size_t Offset = util::size(LocalState{}.itargetsr); + state->itargetsr[i] = this->gicd->itargetsr.words[i + Offset]; + } + + /* Save icfgr. */ + for (size_t i = 0; i < util::size(state->icfgr); ++i) { + constexpr size_t Offset = util::size(LocalState{}.icfgr); + state->icfgr[i] = this->gicd->icfgr[i + Offset]; + } + } + + void KInterruptController::RestoreCoreLocal(const LocalState *state) const { + /* Restore ipriorityr. */ + for (size_t i = 0; i < util::size(state->ipriorityr); ++i) { + constexpr size_t Offset = 0; + this->gicd->ipriorityr.words[i + Offset] = state->ipriorityr[i]; + } + + /* Restore itargetsr. */ + for (size_t i = 0; i < util::size(state->itargetsr); ++i) { + constexpr size_t Offset = 0; + this->gicd->itargetsr.words[i + Offset] = state->itargetsr[i]; + } + + /* Restore icfgr. */ + for (size_t i = 0; i < util::size(state->icfgr); ++i) { + constexpr size_t Offset = 0; + this->gicd->icfgr[i + Offset] = state->icfgr[i]; + } + + /* Restore isenabler. */ + for (size_t i = 0; i < util::size(state->isenabler); ++i) { + constexpr size_t Offset = 0; + this->gicd->icenabler[i + Offset] = 0xFFFFFFFF; + this->gicd->isenabler[i + Offset] = state->isenabler[i]; + } + } + + void KInterruptController::RestoreGlobal(const GlobalState *state) const { + /* Restore ipriorityr. */ + for (size_t i = 0; i < util::size(state->ipriorityr); ++i) { + constexpr size_t Offset = util::size(LocalState{}.ipriorityr); + this->gicd->ipriorityr.words[i + Offset] = state->ipriorityr[i]; + } + + /* Restore itargetsr. */ + for (size_t i = 0; i < util::size(state->itargetsr); ++i) { + constexpr size_t Offset = util::size(LocalState{}.itargetsr); + this->gicd->itargetsr.words[i + Offset] = state->itargetsr[i]; + } + + /* Restore icfgr. */ + for (size_t i = 0; i < util::size(state->icfgr); ++i) { + constexpr size_t Offset = util::size(LocalState{}.icfgr); + this->gicd->icfgr[i + Offset] = state->icfgr[i]; + } + + /* Restore isenabler. */ + for (size_t i = 0; i < util::size(state->isenabler); ++i) { + constexpr size_t Offset = util::size(LocalState{}.isenabler); + this->gicd->icenabler[i + Offset] = 0xFFFFFFFF; + this->gicd->isenabler[i + Offset] = state->isenabler[i]; + } + } + } diff --git a/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_manager.cpp b/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_manager.cpp index 78711d0e1..0386085db 100644 --- a/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_manager.cpp +++ b/libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_manager.cpp @@ -31,6 +31,81 @@ namespace ams::kern::arch::arm64 { this->interrupt_controller.Finalize(core_id); } + void KInterruptManager::Save(s32 core_id) { + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* If on core 0, save the global interrupts. */ + if (core_id == 0) { + MESOSPHERE_ABORT_UNLESS(!s_global_state_saved); + this->interrupt_controller.SaveGlobal(std::addressof(s_global_state)); + s_global_state_saved = true; + } + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Save all local interrupts. */ + MESOSPHERE_ABORT_UNLESS(!this->local_state_saved); + this->interrupt_controller.SaveCoreLocal(std::addressof(this->local_state)); + this->local_state_saved = true; + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Finalize all cores other than core 0. */ + if (core_id != 0) { + this->Finalize(core_id); + } + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Finalize core 0. */ + if (core_id == 0) { + this->Finalize(core_id); + } + } + + void KInterruptManager::Restore(s32 core_id) { + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Initialize core 0. */ + if (core_id == 0) { + this->Initialize(core_id); + } + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Initialize all cores other than core 0. */ + if (core_id != 0) { + this->Initialize(core_id); + } + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Restore all local interrupts. */ + MESOSPHERE_ASSERT(this->local_state_saved); + this->interrupt_controller.RestoreCoreLocal(std::addressof(this->local_state)); + this->local_state_saved = false; + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* If on core 0, restore the global interrupts. */ + if (core_id == 0) { + MESOSPHERE_ASSERT(s_global_state_saved); + this->interrupt_controller.RestoreGlobal(std::addressof(s_global_state)); + s_global_state_saved = false; + } + + /* Ensure all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + } + bool KInterruptManager::OnHandleInterrupt() { /* Get the interrupt id. */ const u32 raw_irq = this->interrupt_controller.GetIrq(); diff --git a/libraries/libmesosphere/source/arch/arm64/kern_k_supervisor_page_table.cpp b/libraries/libmesosphere/source/arch/arm64/kern_k_supervisor_page_table.cpp index 66fac4e51..7abef71f1 100644 --- a/libraries/libmesosphere/source/arch/arm64/kern_k_supervisor_page_table.cpp +++ b/libraries/libmesosphere/source/arch/arm64/kern_k_supervisor_page_table.cpp @@ -19,7 +19,7 @@ namespace ams::kern::arch::arm64 { void KSupervisorPageTable::Initialize(s32 core_id) { /* Get the identity mapping ttbr0. */ - this->ttbr0[core_id] = cpu::GetTtbr0El1(); + this->ttbr0_identity[core_id] = cpu::GetTtbr0El1(); /* Set sctlr_el1 */ cpu::SystemControlRegisterAccessor().SetWxn(true).Store(); diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_device_page_table.cpp b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_device_page_table.cpp index b0122c095..4ada48c89 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_device_page_table.cpp +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_device_page_table.cpp @@ -337,6 +337,8 @@ namespace ams::kern::board::nintendo::nx { KPhysicalAddress g_memory_controller_address; KPhysicalAddress g_reserved_table_phys_addr; KDeviceAsidManager g_asid_manager; + u32 g_saved_page_tables[AsidCount]; + u32 g_saved_asid_registers[ams::svc::DeviceName_Count]; /* Memory controller access functionality. */ void WriteMcRegister(size_t offset, u32 value) { @@ -457,6 +459,61 @@ namespace ams::kern::board::nintendo::nx { /* TODO: Install interrupt handler. */ } + void KDevicePageTable::Lock() { + g_lock.Lock(); + } + + void KDevicePageTable::Unlock() { + g_lock.Unlock(); + } + + void KDevicePageTable::Sleep() { + /* Save all page tables. */ + for (size_t i = 0; i < AsidCount; ++i) { + WriteMcRegister(MC_SMMU_PTB_ASID, i); + SmmuSynchronizationBarrier(); + g_saved_page_tables[i] = ReadMcRegister(MC_SMMU_PTB_DATA); + } + + /* Save all asid registers. */ + for (size_t i = 0; i < ams::svc::DeviceName_Count; ++i) { + g_saved_asid_registers[i] = ReadMcRegister(GetDeviceAsidRegisterOffset(static_cast(i))); + } + } + + void KDevicePageTable::Wakeup() { + /* Synchronize. */ + InvalidatePtc(); + InvalidateTlb(); + SmmuSynchronizationBarrier(); + + /* Disable the SMMU */ + WriteMcRegister(MC_SMMU_CONFIG, 0); + + /* Restore the page tables. */ + for (size_t i = 0; i < AsidCount; ++i) { + WriteMcRegister(MC_SMMU_PTB_ASID, i); + SmmuSynchronizationBarrier(); + WriteMcRegister(MC_SMMU_PTB_DATA, g_saved_page_tables[i]); + } + SmmuSynchronizationBarrier(); + + /* Restore the asid registers. */ + for (size_t i = 0; i < ams::svc::DeviceName_Count; ++i) { + WriteMcRegister(GetDeviceAsidRegisterOffset(static_cast(i)), g_saved_asid_registers[i]); + SmmuSynchronizationBarrier(); + } + + /* Synchronize. */ + InvalidatePtc(); + InvalidateTlb(); + SmmuSynchronizationBarrier(); + + /* Enable the SMMU */ + WriteMcRegister(MC_SMMU_CONFIG, 1); + SmmuSynchronizationBarrier(); + } + /* Member functions. */ Result KDevicePageTable::Initialize(u64 space_address, u64 space_size) { diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.cpp b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.cpp index 3846b1752..0e3f4f602 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.cpp +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.cpp @@ -50,7 +50,7 @@ namespace ams::kern::board::nintendo::nx { u64 pmcr_el0; u64 pmevcntrN_el0[31]; u64 pmevtyperN_el0[31]; - u64 pmcntenset_el1; + u64 pmintenset_el1; u64 pmovsset_el0; u64 pmselr_el0; u64 pmuserenr_el0; @@ -62,13 +62,367 @@ namespace ams::kern::board::nintendo::nx { constexpr s32 SleepManagerThreadPriority = 2; /* Globals for sleep/wake. */ - u64 g_sleep_target_cores; - KLightLock g_request_lock; - KLightLock g_cv_lock; - KLightConditionVariable g_cv; - KPhysicalAddress g_sleep_buffer_phys_addrs[cpu::NumCores]; - alignas(16) u64 g_sleep_buffers[cpu::NumCores][1_KB / sizeof(u64)]; - SavedSystemRegisters g_sleep_system_registers[cpu::NumCores] = {}; + constinit u64 g_sleep_target_cores; + constinit KLightLock g_request_lock; + constinit KLightLock g_cv_lock; + constinit KLightConditionVariable g_cv; + constinit KPhysicalAddress g_sleep_buffer_phys_addrs[cpu::NumCores]; + alignas(1_KB) constinit u64 g_sleep_buffers[cpu::NumCores][1_KB / sizeof(u64)]; + constinit SavedSystemRegisters g_sleep_system_registers[cpu::NumCores] = {}; + + void PowerOnCpu(int core_id, KPhysicalAddress entry_phys_addr, u64 context_id) { + /* Request the secure monitor power on the core. */ + smc::CpuOn(cpu::MultiprocessorAffinityRegisterAccessor().GetCpuOnArgument() | core_id, GetInteger(entry_phys_addr), context_id); + } + + void WaitOtherCpuPowerOff() { + constexpr u64 PmcPhysicalAddress = 0x7000E400; + constexpr u64 APBDEV_PMC_PWRGATE_STATUS = PmcPhysicalAddress + 0x38; + + constexpr u32 PWRGATE_STATUS_CE123_MASK = ((1u << 3) - 1) << 9; + + u32 value; + do { + bool res = smc::ReadWriteRegister(std::addressof(value), APBDEV_PMC_PWRGATE_STATUS, 0, 0); + MESOSPHERE_ASSERT(res); + } while ((value & PWRGATE_STATUS_CE123_MASK) != 0); + } + + void SavedSystemRegisters::Save() { + /* Save system registers. */ + this->ttbr0_el1 = cpu::GetTtbr0El1(); + this->tcr_el1 = cpu::GetTcrEl1(); + this->tpidr_el0 = cpu::GetTpidrEl0(); + this->elr_el1 = cpu::GetElrEl1(); + this->sp_el0 = cpu::GetSpEl0(); + this->spsr_el1 = cpu::GetSpsrEl1(); + this->daif = cpu::GetDaif(); + this->cpacr_el1 = cpu::GetCpacrEl1(); + this->vbar_el1 = cpu::GetVbarEl1(); + this->csselr_el1 = cpu::GetCsselrEl1(); + this->cntp_ctl_el0 = cpu::GetCntpCtlEl0(); + this->cntp_cval_el0 = cpu::GetCntpCvalEl0(); + this->cntkctl_el1 = cpu::GetCntkCtlEl1(); + this->tpidrro_el0 = cpu::GetTpidrRoEl0(); + + /* Save pmu registers. */ + { + /* Get and clear pmcr_el0 */ + this->pmcr_el0 = cpu::GetPmcrEl0(); + cpu::SetPmcrEl0(0); + cpu::EnsureInstructionConsistency(); + + /* Save other pmu registers. */ + this->pmuserenr_el0 = cpu::GetPmUserEnrEl0(); + this->pmselr_el0 = cpu::GetPmSelrEl0(); + this->pmccfiltr_el0 = cpu::GetPmcCfiltrEl0(); + this->pmcntenset_el0 = cpu::GetPmCntEnSetEl0(); + this->pmintenset_el1 = cpu::GetPmIntEnSetEl1(); + this->pmovsset_el0 = cpu::GetPmOvsSetEl0(); + this->pmccntr_el0 = cpu::GetPmcCntrEl0(); + + switch (cpu::PerformanceMonitorsControlRegisterAccessor(this->pmcr_el0).GetN()) { + #define HANDLE_PMU_CASE(N) \ + case (N+1): \ + this->pmevcntrN_el0 [ N ] = cpu::GetPmevCntr##N##El0(); \ + this->pmevtyperN_el0[ N ] = cpu::GetPmevTyper##N##El0(); \ + [[fallthrough]] + + HANDLE_PMU_CASE(30); + HANDLE_PMU_CASE(29); + HANDLE_PMU_CASE(28); + HANDLE_PMU_CASE(27); + HANDLE_PMU_CASE(26); + HANDLE_PMU_CASE(25); + HANDLE_PMU_CASE(24); + HANDLE_PMU_CASE(23); + HANDLE_PMU_CASE(22); + HANDLE_PMU_CASE(21); + HANDLE_PMU_CASE(20); + HANDLE_PMU_CASE(19); + HANDLE_PMU_CASE(18); + HANDLE_PMU_CASE(17); + HANDLE_PMU_CASE(16); + HANDLE_PMU_CASE(15); + HANDLE_PMU_CASE(14); + HANDLE_PMU_CASE(13); + HANDLE_PMU_CASE(12); + HANDLE_PMU_CASE(11); + HANDLE_PMU_CASE(10); + HANDLE_PMU_CASE( 9); + HANDLE_PMU_CASE( 8); + HANDLE_PMU_CASE( 7); + HANDLE_PMU_CASE( 6); + HANDLE_PMU_CASE( 5); + HANDLE_PMU_CASE( 4); + HANDLE_PMU_CASE( 3); + HANDLE_PMU_CASE( 2); + HANDLE_PMU_CASE( 1); + HANDLE_PMU_CASE( 0); + + #undef HANDLE_PMU_CASE + case 0: + default: + break; + } + } + + /* Save debug registers. */ + const u64 dfr0 = cpu::GetIdAa64Dfr0El1(); + + this->mdscr_el1 = cpu::GetMdscrEl1(); + this->contextidr_el1 = cpu::GetContextidrEl1(); + + /* Save watchpoints. */ + switch (cpu::DebugFeatureRegisterAccessor(dfr0).GetNumWatchpoints()) { + #define HANDLE_DBG_CASE(N) \ + case N: \ + this->dbgwcrN_el1[ N ] = cpu::GetDbgWcr##N##El1(); \ + this->dbgwvrN_el1[ N ] = cpu::GetDbgWvr##N##El1(); \ + [[fallthrough]] + + HANDLE_DBG_CASE(15); + HANDLE_DBG_CASE(14); + HANDLE_DBG_CASE(13); + HANDLE_DBG_CASE(12); + HANDLE_DBG_CASE(11); + HANDLE_DBG_CASE(10); + HANDLE_DBG_CASE( 9); + HANDLE_DBG_CASE( 8); + HANDLE_DBG_CASE( 7); + HANDLE_DBG_CASE( 6); + HANDLE_DBG_CASE( 5); + HANDLE_DBG_CASE( 4); + HANDLE_DBG_CASE( 3); + HANDLE_DBG_CASE( 2); + + #undef HANDLE_DBG_CASE + + case 1: + this->dbgwcrN_el1[1] = cpu::GetDbgWcr1El1(); + this->dbgwvrN_el1[1] = cpu::GetDbgWvr1El1(); + this->dbgwcrN_el1[0] = cpu::GetDbgWcr0El1(); + this->dbgwvrN_el1[0] = cpu::GetDbgWvr0El1(); + [[fallthrough]]; + default: + break; + } + + /* Save breakpoints. */ + switch (cpu::DebugFeatureRegisterAccessor(dfr0).GetNumBreakpoints()) { + #define HANDLE_DBG_CASE(N) \ + case N: \ + this->dbgbcrN_el1[ N ] = cpu::GetDbgBcr##N##El1(); \ + this->dbgbvrN_el1[ N ] = cpu::GetDbgBvr##N##El1(); \ + [[fallthrough]] + + HANDLE_DBG_CASE(15); + HANDLE_DBG_CASE(14); + HANDLE_DBG_CASE(13); + HANDLE_DBG_CASE(12); + HANDLE_DBG_CASE(11); + HANDLE_DBG_CASE(10); + HANDLE_DBG_CASE( 9); + HANDLE_DBG_CASE( 8); + HANDLE_DBG_CASE( 7); + HANDLE_DBG_CASE( 6); + HANDLE_DBG_CASE( 5); + HANDLE_DBG_CASE( 4); + HANDLE_DBG_CASE( 3); + HANDLE_DBG_CASE( 2); + + #undef HANDLE_DBG_CASE + + case 1: + this->dbgbcrN_el1[1] = cpu::GetDbgBcr1El1(); + this->dbgbvrN_el1[1] = cpu::GetDbgBvr1El1(); + [[fallthrough]]; + default: + break; + } + + this->dbgbcrN_el1[0] = cpu::GetDbgBcr0El1(); + this->dbgbvrN_el1[0] = cpu::GetDbgBvr0El1(); + cpu::EnsureInstructionConsistency(); + + /* Clear mdscr_el1. */ + cpu::SetMdscrEl1(0); + cpu::EnsureInstructionConsistency(); + } + + void SavedSystemRegisters::Restore() const { + /* Restore debug registers. */ + const u64 dfr0 = cpu::GetIdAa64Dfr0El1(); + cpu::EnsureInstructionConsistency(); + + cpu::SetMdscrEl1(0); + cpu::EnsureInstructionConsistency(); + + cpu::SetOslarEl1(0); + cpu::EnsureInstructionConsistency(); + + /* Restore watchpoints. */ + switch (cpu::DebugFeatureRegisterAccessor(dfr0).GetNumWatchpoints()) { + #define HANDLE_DBG_CASE(N) \ + case N: \ + cpu::SetDbgWcr##N##El1(this->dbgwcrN_el1[ N ]); \ + cpu::SetDbgWvr##N##El1(this->dbgwvrN_el1[ N ]); \ + [[fallthrough]] + + HANDLE_DBG_CASE(15); + HANDLE_DBG_CASE(14); + HANDLE_DBG_CASE(13); + HANDLE_DBG_CASE(12); + HANDLE_DBG_CASE(11); + HANDLE_DBG_CASE(10); + HANDLE_DBG_CASE( 9); + HANDLE_DBG_CASE( 8); + HANDLE_DBG_CASE( 7); + HANDLE_DBG_CASE( 6); + HANDLE_DBG_CASE( 5); + HANDLE_DBG_CASE( 4); + HANDLE_DBG_CASE( 3); + HANDLE_DBG_CASE( 2); + + #undef HANDLE_DBG_CASE + + case 1: + cpu::SetDbgWcr1El1(this->dbgwcrN_el1[1]); + cpu::SetDbgWvr1El1(this->dbgwvrN_el1[1]); + cpu::SetDbgWcr0El1(this->dbgwcrN_el1[0]); + cpu::SetDbgWvr0El1(this->dbgwvrN_el1[0]); + [[fallthrough]]; + default: + break; + } + + /* Restore breakpoints. */ + switch (cpu::DebugFeatureRegisterAccessor(dfr0).GetNumBreakpoints()) { + #define HANDLE_DBG_CASE(N) \ + case N: \ + cpu::SetDbgBcr##N##El1(this->dbgbcrN_el1[ N ]); \ + cpu::SetDbgBvr##N##El1(this->dbgbvrN_el1[ N ]); \ + [[fallthrough]] + + HANDLE_DBG_CASE(15); + HANDLE_DBG_CASE(14); + HANDLE_DBG_CASE(13); + HANDLE_DBG_CASE(12); + HANDLE_DBG_CASE(11); + HANDLE_DBG_CASE(10); + HANDLE_DBG_CASE( 9); + HANDLE_DBG_CASE( 8); + HANDLE_DBG_CASE( 7); + HANDLE_DBG_CASE( 6); + HANDLE_DBG_CASE( 5); + HANDLE_DBG_CASE( 4); + HANDLE_DBG_CASE( 3); + HANDLE_DBG_CASE( 2); + + #undef HANDLE_DBG_CASE + + case 1: + cpu::SetDbgBcr1El1(this->dbgbcrN_el1[1]); + cpu::SetDbgBvr1El1(this->dbgbvrN_el1[1]); + [[fallthrough]]; + default: + break; + } + + cpu::SetDbgBcr0El1(this->dbgbcrN_el1[0]); + cpu::SetDbgBvr0El1(this->dbgbvrN_el1[0]); + cpu::EnsureInstructionConsistency(); + + cpu::SetContextidrEl1(this->contextidr_el1); + cpu::EnsureInstructionConsistency(); + + cpu::SetMdscrEl1(this->mdscr_el1); + cpu::EnsureInstructionConsistency(); + + /* Restore pmu registers. */ + cpu::SetPmUserEnrEl0(0); + cpu::PerformanceMonitorsControlRegisterAccessor().SetEventCounterReset(true).SetCycleCounterReset(true).Store(); + cpu::SetPmOvsClrEl0(static_cast(static_cast(~u32()))); + cpu::SetPmIntEnClrEl1(static_cast(static_cast(~u32()))); + cpu::SetPmCntEnClrEl0(static_cast(static_cast(~u32()))); + + switch (cpu::PerformanceMonitorsControlRegisterAccessor(this->pmcr_el0).GetN()) { + #define HANDLE_PMU_CASE(N) \ + case (N+1): \ + cpu::SetPmevCntr##N##El0 (this->pmevcntrN_el0 [ N ]); \ + cpu::SetPmevTyper##N##El0(this->pmevtyperN_el0[ N ]); \ + [[fallthrough]] + + HANDLE_PMU_CASE(30); + HANDLE_PMU_CASE(29); + HANDLE_PMU_CASE(28); + HANDLE_PMU_CASE(27); + HANDLE_PMU_CASE(26); + HANDLE_PMU_CASE(25); + HANDLE_PMU_CASE(24); + HANDLE_PMU_CASE(23); + HANDLE_PMU_CASE(22); + HANDLE_PMU_CASE(21); + HANDLE_PMU_CASE(20); + HANDLE_PMU_CASE(19); + HANDLE_PMU_CASE(18); + HANDLE_PMU_CASE(17); + HANDLE_PMU_CASE(16); + HANDLE_PMU_CASE(15); + HANDLE_PMU_CASE(14); + HANDLE_PMU_CASE(13); + HANDLE_PMU_CASE(12); + HANDLE_PMU_CASE(11); + HANDLE_PMU_CASE(10); + HANDLE_PMU_CASE( 9); + HANDLE_PMU_CASE( 8); + HANDLE_PMU_CASE( 7); + HANDLE_PMU_CASE( 6); + HANDLE_PMU_CASE( 5); + HANDLE_PMU_CASE( 4); + HANDLE_PMU_CASE( 3); + HANDLE_PMU_CASE( 2); + HANDLE_PMU_CASE( 1); + HANDLE_PMU_CASE( 0); + + #undef HANDLE_PMU_CASE + case 0: + default: + break; + } + + cpu::SetPmUserEnrEl0 (this->pmuserenr_el0); + cpu::SetPmSelrEl0 (this->pmselr_el0); + cpu::SetPmcCfiltrEl0 (this->pmccfiltr_el0); + cpu::SetPmCntEnSetEl0(this->pmcntenset_el0); + cpu::SetPmIntEnSetEl1(this->pmintenset_el1); + cpu::SetPmOvsSetEl0 (this->pmovsset_el0); + cpu::SetPmcCntrEl0 (this->pmccntr_el0); + cpu::EnsureInstructionConsistency(); + + cpu::SetPmcrEl0(this->pmcr_el0); + cpu::EnsureInstructionConsistency(); + + /* Restore system registers. */ + cpu::SetTtbr0El1 (this->ttbr0_el1); + cpu::SetTcrEl1 (this->tcr_el1); + cpu::SetTpidrEl0 (this->tpidr_el0); + cpu::SetElrEl1 (this->elr_el1); + cpu::SetSpEl0 (this->sp_el0); + cpu::SetSpsrEl1 (this->spsr_el1); + cpu::SetDaif (this->daif); + cpu::SetCpacrEl1 (this->cpacr_el1); + cpu::SetVbarEl1 (this->vbar_el1); + cpu::SetCsselrEl1 (this->csselr_el1); + cpu::SetCntpCtlEl0 (this->cntp_ctl_el0); + cpu::SetCntpCvalEl0(this->cntp_cval_el0); + cpu::SetCntkCtlEl1 (this->cntkctl_el1); + cpu::SetTpidrRoEl0 (this->tpidrro_el0); + cpu::EnsureInstructionConsistency(); + + /* Invalidate the entire tlb. */ + cpu::InvalidateEntireTlb(); + } } @@ -95,8 +449,8 @@ namespace ams::kern::board::nintendo::nx { void KSleepManager::SleepSystem() { /* Ensure device mappings are not modified during sleep. */ - MESOSPHERE_TODO("KDevicePageTable::Lock();"); - ON_SCOPE_EXIT { MESOSPHERE_TODO("KDevicePageTable::Unlock();"); }; + KDevicePageTable::Lock(); + ON_SCOPE_EXIT { KDevicePageTable::Unlock(); }; /* Request that the system sleep. */ { @@ -107,7 +461,7 @@ namespace ams::kern::board::nintendo::nx { KScopedLightLock lk(g_cv_lock); MESOSPHERE_ABORT_UNLESS(g_sleep_target_cores == 0); - g_sleep_target_cores = (1ul << (cpu::NumCores - 1)); + g_sleep_target_cores = (1ul << cpu::NumCores) - 1; g_cv.Broadcast(); while (g_sleep_target_cores != 0) { @@ -140,9 +494,106 @@ namespace ams::kern::board::nintendo::nx { } } - MESOSPHERE_TODO("Implement Sleep/Wake"); - (void)(g_sleep_system_registers[core_id]); - (void)(sleep_buffer_phys_addr); + /* Perform Sleep/Wake sequence. */ + { + /* Disable interrupts. */ + KScopedInterruptDisable di; + + /* Save the system registers for the current core. */ + g_sleep_system_registers[core_id].Save(); + + /* Change the translation tables to use the kernel table. */ + { + /* Get the current value of the translation control register. */ + const u64 tcr = cpu::GetTcrEl1(); + + /* Disable translation table walks on tlb miss. */ + cpu::TranslationControlRegisterAccessor(tcr).SetEpd0(true).Store(); + cpu::EnsureInstructionConsistency(); + + /* Change the translation table base (ttbr0) to use the kernel table. */ + cpu::SetTtbr0El1(Kernel::GetKernelPageTable().GetIdentityMapTtbr0(core_id)); + cpu::EnsureInstructionConsistency(); + + /* Enable translation table walks on tlb miss. */ + cpu::TranslationControlRegisterAccessor(tcr).SetEpd0(false).Store(); + cpu::EnsureInstructionConsistency(); + } + + /* Invalidate the entire tlb. */ + cpu::InvalidateEntireTlb(); + + /* Ensure that all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* If on core 0, put the device page tables to sleep. */ + if (core_id == 0) { + KDevicePageTable::Sleep(); + } + + /* Ensure that all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Save the interrupt manager's state. */ + Kernel::GetInterruptManager().Save(core_id); + + /* Ensure that all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Log that the core is going to sleep. */ + MESOSPHERE_LOG("Core[%d]: Going to sleep, buffer = %010lx\n", core_id, GetInteger(sleep_buffer_phys_addr)); + + /* If we're on a core other than zero, we can just invoke the sleep handler. */ + if (core_id != 0) { + CpuSleepHandler(GetInteger(sleep_buffer_phys_addr), GetInteger(resume_entry_phys_addr)); + } else { + /* Wait for all other cores to be powered off. */ + WaitOtherCpuPowerOff(); + + /* Log that we're about to enter SC7. */ + MESOSPHERE_LOG("Entering SC7\n"); + + /* Save the debug log state. */ + KDebugLog::Save(); + + /* Invoke the sleep handler. */ + CpuSleepHandler(GetInteger(sleep_buffer_phys_addr), GetInteger(resume_entry_phys_addr)); + + /* Restore the debug log state. */ + KDebugLog::Restore(); + + /* Log that we're about to exit SC7. */ + MESOSPHERE_LOG("Exiting SC7\n"); + + /* Wake up the other cores. */ + for (s32 i = 1; i < static_cast(cpu::NumCores); ++i) { + PowerOnCpu(i, resume_entry_phys_addr, GetInteger(g_sleep_buffer_phys_addrs[i])); + } + } + + /* Log that the core is waking from sleep. */ + MESOSPHERE_LOG("Core[%d]: Woke from sleep.\n", core_id); + + /* Ensure that all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Restore the interrupt manager's state. */ + Kernel::GetInterruptManager().Restore(core_id); + + /* Ensure that all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* If on core 0, wake up the device page tables. */ + if (core_id == 0) { + KDevicePageTable::Wakeup(); + } + + /* Ensure that all cores get to this point before continuing. */ + cpu::SynchronizeAllCores(); + + /* Restore the system registers for the current core. */ + g_sleep_system_registers[core_id].Restore(); + } /* Signal request completed. */ { diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.hpp b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.hpp index 1cc500c4d..f39ce7b67 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.hpp +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager.hpp @@ -23,6 +23,8 @@ namespace ams::kern::board::nintendo::nx { static void CpuSleepHandler(uintptr_t arg, uintptr_t entry); static void ResumeEntry(uintptr_t arg); + static void InvalidateDataCacheForResumeEntry(uintptr_t level); + static void ProcessRequests(uintptr_t buffer); public: static void Initialize(); diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager_asm.s b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager_asm.s index 1c4a014cd..210155730 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager_asm.s +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_sleep_manager_asm.s @@ -14,10 +14,328 @@ * along with this program. If not, see . */ + +/* For some reason GAS doesn't know about it, even with .cpu cortex-a57 */ +#define cpuactlr_el1 s3_1_c15_c2_0 +#define cpuectlr_el1 s3_1_c15_c2_1 + +#define LOAD_IMMEDIATE_32(reg, val) \ + mov reg, #(((val) >> 0x00) & 0xFFFF); \ + movk reg, #(((val) >> 0x10) & 0xFFFF), lsl#16 + +/* ams::kern::board::nintendo::nx::KSleepManager::CpuSleepHandler(uintptr_t arg, uintptr_t entry) */ +.section .sleep._ZN3ams4kern5board8nintendo2nx13KSleepManager15CpuSleepHandlerEmm, "ax", %progbits +.global _ZN3ams4kern5board8nintendo2nx13KSleepManager15CpuSleepHandlerEmm +.type _ZN3ams4kern5board8nintendo2nx13KSleepManager15CpuSleepHandlerEmm, %function +_ZN3ams4kern5board8nintendo2nx13KSleepManager15CpuSleepHandlerEmm: + /* Save arguments. */ + mov x17, x0 + mov x18, x1 + + /* Enable access to FPU registers. */ + mrs x1, cpacr_el1 + orr x1, x1, #0x100000 + msr cpacr_el1, x1 + dsb sy + isb + + /* Save callee-save registers. */ + stp x19, x20, [x0], #0x10 + stp x21, x22, [x0], #0x10 + stp x23, x24, [x0], #0x10 + stp x25, x26, [x0], #0x10 + stp x27, x28, [x0], #0x10 + stp x29, x30, [x0], #0x10 + + /* Save stack pointer. */ + mov x1, sp + str x1, [x0], #8 + + /* Save fpcr/fpsr. */ + mrs x1, fpcr + mrs x2, fpsr + stp w1, w2, [x0], #8 + + /* Save the floating point registers. */ + stp q0, q1, [x0], #0x20 + stp q2, q3, [x0], #0x20 + stp q4, q5, [x0], #0x20 + stp q6, q7, [x0], #0x20 + stp q8, q9, [x0], #0x20 + stp q10, q11, [x0], #0x20 + stp q12, q13, [x0], #0x20 + stp q14, q15, [x0], #0x20 + stp q16, q17, [x0], #0x20 + stp q28, q19, [x0], #0x20 + stp q20, q21, [x0], #0x20 + stp q22, q23, [x0], #0x20 + stp q24, q25, [x0], #0x20 + stp q26, q27, [x0], #0x20 + stp q28, q29, [x0], #0x20 + stp q30, q31, [x0], #0x20 + + /* Save cpuactlr/cpuectlr. */ + mrs x1, cpuectlr_el1 + mrs x2, cpuactlr_el1 + stp x1, x2, [x0], #0x10 + + /* Save ttbr0/ttbr1. */ + mrs x1, ttbr0_el1 + mrs x2, ttbr1_el1 + stp x1, x2, [x0], #0x10 + + /* Save tcr/mair. */ + mrs x1, tcr_el1 + mrs x2, mair_el1 + stp x1, x2, [x0], #0x10 + + /* Save sctlr/tpidr. */ + mrs x1, sctlr_el1 + mrs x2, tpidr_el1 + stp x1, x2, [x0], #0x10 + + /* Save the virtual resumption entrypoint. */ + adr x1, 77f + stp x1, xzr, [x0], #0x10 + + /* Get the current core id. */ + mrs x0, mpidr_el1 + and x0, x0, #0xFF + + /* If we're on core 0, suspend. */ + cbz x0, 1f + + /* Otherwise, power off. */ + LOAD_IMMEDIATE_32(x0, 0x84000002) + smc #1 +0: b 0b + +1: /* Suspend. */ + LOAD_IMMEDIATE_32(x0, 0xC4000001) + LOAD_IMMEDIATE_32(x1, 0x0201001B) + mov x2, x18 + mov x3, x17 + smc #1 +0: b 0b + /* ams::kern::board::nintendo::nx::KSleepManager::ResumeEntry(uintptr_t arg) */ -.section .text._ZN3ams4kern5board8nintendo2nx13KSleepManager11ResumeEntryEm, "ax", %progbits +.section .sleep._ZN3ams4kern5board8nintendo2nx13KSleepManager11ResumeEntryEm, "ax", %progbits .global _ZN3ams4kern5board8nintendo2nx13KSleepManager11ResumeEntryEm .type _ZN3ams4kern5board8nintendo2nx13KSleepManager11ResumeEntryEm, %function _ZN3ams4kern5board8nintendo2nx13KSleepManager11ResumeEntryEm: - /* TODO: Implement a real function here. */ - brk 1000 \ No newline at end of file + /* Mask interrupts. */ + msr daifset, #0xF + + /* Save the argument. */ + mov x21, x0 + + /* Check that we're at the correct exception level. */ + mrs x0, currentel + + /* Check if we're EL1. */ + cmp x0, #0x4 + b.eq 3f + + /* Check if we're EL2. */ + cmp x0, #0x8 + b.eq 2f + +1: /* We're running at EL3. */ + b 1b + +2: /* We're running at EL2. */ + b 2b + +3: /* We're running at EL1. */ + + /* Invalidate the L1 cache. */ + mov x0, #0 + bl _ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm + + /* Get the current core id. */ + mrs x0, mpidr_el1 + and x0, x0, #0xFF + + /* If we're on core0, we want to invalidate the L2 cache. */ + cbnz x0, 4f + + mov x0, #1 + bl _ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm + +4: /* Invalidate the L1 cache. */ + mov x0, #0 + bl _ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm + + /* Invalidate the instruction cache. */ + ic ialluis + dsb sy + isb + + /* Invalidate the entire tlb. */ + tlbi vmalle1is + dsb sy + isb + + /* Switch to sp 1. */ + msr spsel, #1 + + /* Prepare to restore the saved context. */ + mov x0, x21 + + /* Enable access to FPU registers. */ + mrs x1, cpacr_el1 + orr x1, x1, #0x100000 + msr cpacr_el1, x1 + dsb sy + isb + + /* Restore callee-save registers. */ + ldp x19, x20, [x0], #0x10 + ldp x21, x22, [x0], #0x10 + ldp x23, x24, [x0], #0x10 + ldp x25, x26, [x0], #0x10 + ldp x27, x28, [x0], #0x10 + ldp x29, x30, [x0], #0x10 + + /* Restore stack pointer. */ + ldr x1, [x0], #8 + mov sp, x1 + + /* Restore fpcr/fpsr. */ + ldp w1, w2, [x0], #8 + msr fpcr, x1 + msr fpsr, x2 + + /* Restore the floating point registers. */ + ldp q0, q1, [x0], #0x20 + ldp q2, q3, [x0], #0x20 + ldp q4, q5, [x0], #0x20 + ldp q6, q7, [x0], #0x20 + ldp q8, q9, [x0], #0x20 + ldp q10, q11, [x0], #0x20 + ldp q12, q13, [x0], #0x20 + ldp q14, q15, [x0], #0x20 + ldp q16, q17, [x0], #0x20 + ldp q28, q19, [x0], #0x20 + ldp q20, q21, [x0], #0x20 + ldp q22, q23, [x0], #0x20 + ldp q24, q25, [x0], #0x20 + ldp q26, q27, [x0], #0x20 + ldp q28, q29, [x0], #0x20 + ldp q30, q31, [x0], #0x20 + + /* Restore cpuactlr/cpuectlr. */ + ldp x1, x2, [x0], #0x10 + mrs x3, cpuectlr_el1 + cmp x1, x3 +5: b.ne 5b + mrs x3, cpuactlr_el1 + cmp x2, x3 +6: b.ne 6b + + /* Restore ttbr0/ttbr1. */ + ldp x1, x2, [x0], #0x10 + msr ttbr0_el1, x1 + msr ttbr1_el1, x2 + + /* Restore tcr/mair. */ + ldp x1, x2, [x0], #0x10 + msr tcr_el1, x1 + msr mair_el1, x2 + + /* Get sctlr, tpidr, and the entrypoint. */ + ldp x1, x2, [x0], #0x10 + ldp x3, xzr, [x0], #0x10 + + /* Set the global context back into x18/tpidr. */ + msr tpidr_el1, x2 + mov x18, x2 + dsb sy + isb + + /* Restore sctlr with the wxn bit cleared. */ + bic x2, x1, #0x80000 + msr sctlr_el1, x2 + dsb sy + isb + + /* Jump to the entrypoint. */ + br x3 + +77: /* Virtual resumption entrypoint. */ + + /* Restore sctlr. */ + msr sctlr_el1, x1 + dsb sy + isb + + ret + +/* ams::kern::board::nintendo::nx::KSleepManager::InvalidateDataCacheForResumeEntry(uintptr_t level) */ +.section .sleep._ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm, "ax", %progbits +.global _ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm +.type _ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm, %function +_ZN3ams4kern5board8nintendo2nx13KSleepManager33InvalidateDataCacheForResumeEntryEm: + /* const u64 level_sel_value = level << 1; */ + lsl x8, x0, #1 + + /* cpu::SetCsselrEl1(level_sel_value); */ + msr csselr_el1, x8 + + /* cpu::InstructionMemoryBarrier(); */ + isb + + /* CacheSizeIdAccessor ccsidr_el1; */ + mrs x13, ccsidr_el1 + + /* const int num_ways = ccsidr_el1.GetAssociativity(); */ + ubfx w10, w13, #3, #0xA + + /* const int line_size = ccsidr_el1.GetLineSize(); */ + and w11, w13, #7 + + /* const int num_sets = ccsidr_el1.GetNumberOfSets(); */ + ubfx w13, w13, #0xD, #0xF + + /* int way = 0; */ + mov w9, wzr + + /* const u64 set_shift = static_cast(line_size + 4); */ + add w11, w11, #4 + + /* const u64 way_shift = static_cast(__builtin_clz(num_ways)); */ + clz w12, w10 + + +0: /* do { */ + /* int set = 0; */ + mov w14, wzr + + /* const u64 way_value = (static_cast(way) << way_shift); */ + lsl w15, w9, w12 + +1: /* do { */ + + /* const u64 isw_value = (static_cast(set) << set_shift) | way_value | level_sel_value; */ + lsl w16, w14, w11 + orr w16, w16, w15 + sxtw x16, w16 + orr x16, x16, x8 + + /* __asm__ __volatile__("dc isw, %0" :: "r"(isw_value) : "memory"); */ + dc isw, x16 + + /* while (set <= num_sets); */ + cmp w13, w14 + add w14, w14, #1 + b.ne 1b + + /* while (way <= num_ways); */ + cmp w9, w10 + add w9, w9, #1 + b.ne 0b + + /* cpu::EnsureInstructionConsistency(); */ + dsb sy + isb + ret diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_system_control.cpp b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_system_control.cpp index 4adaa63f2..2fa8406b1 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_k_system_control.cpp +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_k_system_control.cpp @@ -423,6 +423,8 @@ namespace ams::kern::board::nintendo::nx { /* Display a panic screen via secure monitor. */ smc::Panic(0xF00); } + u32 dummy; + smc::init::ReadWriteRegister(std::addressof(dummy), 0x7000E400, 0x10, 0x10); while (true) { /* ... */ } } diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.cpp b/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.cpp index 3ae375069..9405d8a67 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.cpp +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.cpp @@ -201,6 +201,12 @@ namespace ams::kern::board::nintendo::nx::smc { MESOSPHERE_ABORT_UNLESS((static_cast(args.x[0]) == SmcResult::Success)); } + void CpuOn(u64 core_id, uintptr_t entrypoint, uintptr_t arg) { + SecureMonitorArguments args = { FunctionId_CpuOn, core_id, static_cast(entrypoint), static_cast(arg) }; + CallPrivilegedSecureMonitorFunction(args); + MESOSPHERE_ABORT_UNLESS((static_cast(args.x[0]) == SmcResult::Success)); + } + void GenerateRandomBytes(void *dst, size_t size) { /* Setup for call. */ SecureMonitorArguments args = { FunctionId_GenerateRandomBytes, size }; diff --git a/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.hpp b/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.hpp index 72abe5dee..fe3e7634f 100644 --- a/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.hpp +++ b/libraries/libmesosphere/source/board/nintendo/nx/kern_secure_monitor.hpp @@ -89,6 +89,8 @@ namespace ams::kern::board::nintendo::nx::smc { bool ReadWriteRegister(u32 *out, ams::svc::PhysicalAddress address, u32 mask, u32 value); void ConfigureCarveout(size_t which, uintptr_t address, size_t size); + void CpuOn(u64 core_id, uintptr_t entrypoint, uintptr_t arg); + void NORETURN Panic(u32 color); void CallSecureMonitorFromUser(ams::svc::lp64::SecureMonitorArguments *args); diff --git a/libraries/libmesosphere/source/kern_debug_log.cpp b/libraries/libmesosphere/source/kern_debug_log.cpp index bf06f82d2..6288f1df2 100644 --- a/libraries/libmesosphere/source/kern_debug_log.cpp +++ b/libraries/libmesosphere/source/kern_debug_log.cpp @@ -503,4 +503,22 @@ namespace ams::kern { return ResultSuccess(); } + void KDebugLog::Save() { + if (KTargetSystem::IsDebugLoggingEnabled()) { + KScopedInterruptDisable di; + KScopedSpinLock lk(g_debug_log_lock); + + KDebugLogImpl::Save(); + } + } + + void KDebugLog::Restore() { + if (KTargetSystem::IsDebugLoggingEnabled()) { + KScopedInterruptDisable di; + KScopedSpinLock lk(g_debug_log_lock); + + KDebugLogImpl::Restore(); + } + } + } diff --git a/libraries/libmesosphere/source/kern_debug_log_impl.board.nintendo_nx.cpp b/libraries/libmesosphere/source/kern_debug_log_impl.board.nintendo_nx.cpp index e90cbc70f..c3be814f5 100644 --- a/libraries/libmesosphere/source/kern_debug_log_impl.board.nintendo_nx.cpp +++ b/libraries/libmesosphere/source/kern_debug_log_impl.board.nintendo_nx.cpp @@ -28,7 +28,7 @@ namespace ams::kern { UartRegister_LSR = 5, - UartRegister_IRSA_CSR = 8, + UartRegister_IRDA_CSR = 8, UartRegister_DLL = 0, UartRegister_DLH = 1, @@ -36,6 +36,8 @@ namespace ams::kern { KVirtualAddress g_uart_address = 0; + constinit u32 g_saved_registers[5]; + NOINLINE u32 ReadUartRegister(UartRegister which) { return GetPointer(g_uart_address)[which]; } @@ -75,7 +77,7 @@ namespace ams::kern { /* Configure the FIFO to be enabled and clear receive. */ WriteUartRegister(UartRegister_FCR, 0x03); - WriteUartRegister(UartRegister_IRSA_CSR, 0x02); + WriteUartRegister(UartRegister_IRDA_CSR, 0x02); ReadUartRegister(UartRegister_FCR); return true; @@ -96,4 +98,44 @@ namespace ams::kern { } } + void KDebugLogImpl::Save() { + /* Save LCR, IER, FCR. */ + g_saved_registers[0] = ReadUartRegister(UartRegister_LCR); + g_saved_registers[1] = ReadUartRegister(UartRegister_IER); + g_saved_registers[2] = ReadUartRegister(UartRegister_FCR); + + /* Set Divisor Latch Access bit, to allow access to DLL/DLH */ + WriteUartRegister(UartRegister_LCR, 0x80); + ReadUartRegister(UartRegister_LCR); + + /* Save DLL/DLH. */ + g_saved_registers[3] = ReadUartRegister(UartRegister_DLL); + g_saved_registers[4] = ReadUartRegister(UartRegister_DLH); + + /* Restore Divisor Latch Access bit. */ + WriteUartRegister(UartRegister_LCR, g_saved_registers[0]); + ReadUartRegister(UartRegister_LCR); + } + + void KDebugLogImpl::Restore() { + /* Set Divisor Latch Access bit, to allow access to DLL/DLH */ + WriteUartRegister(UartRegister_LCR, 0x80); + ReadUartRegister(UartRegister_LCR); + + /* Restore DLL/DLH. */ + WriteUartRegister(UartRegister_DLL, g_saved_registers[3]); + WriteUartRegister(UartRegister_DLH, g_saved_registers[4]); + ReadUartRegister(UartRegister_DLH); + + /* Restore Divisor Latch Access bit. */ + WriteUartRegister(UartRegister_LCR, g_saved_registers[0]); + ReadUartRegister(UartRegister_LCR); + + /* Restore IER and FCR. */ + WriteUartRegister(UartRegister_IER, g_saved_registers[1]); + WriteUartRegister(UartRegister_FCR, g_saved_registers[2] | 2); + WriteUartRegister(UartRegister_IRDA_CSR, 0x02); + ReadUartRegister(UartRegister_FCR); + } + } diff --git a/libraries/libmesosphere/source/kern_debug_log_impl.hpp b/libraries/libmesosphere/source/kern_debug_log_impl.hpp index 1a278a03c..b2b7aa049 100644 --- a/libraries/libmesosphere/source/kern_debug_log_impl.hpp +++ b/libraries/libmesosphere/source/kern_debug_log_impl.hpp @@ -23,6 +23,10 @@ namespace ams::kern { static NOINLINE bool Initialize(); static NOINLINE void PutChar(char c); static NOINLINE void Flush(); + + /* Functionality for preserving across sleep. */ + static NOINLINE void Save(); + static NOINLINE void Restore(); }; } diff --git a/libraries/libmesosphere/source/svc/kern_svc_power_management.cpp b/libraries/libmesosphere/source/svc/kern_svc_power_management.cpp index 41b0b8e87..34cbd32cb 100644 --- a/libraries/libmesosphere/source/svc/kern_svc_power_management.cpp +++ b/libraries/libmesosphere/source/svc/kern_svc_power_management.cpp @@ -21,20 +21,22 @@ namespace ams::kern::svc { namespace { - + void SleepSystem() { + return KSystemControl::SleepSystem(); + } } /* ============================= 64 ABI ============================= */ void SleepSystem64() { - MESOSPHERE_PANIC("Stubbed SvcSleepSystem64 was called."); + return SleepSystem(); } /* ============================= 64From32 ABI ============================= */ void SleepSystem64From32() { - MESOSPHERE_PANIC("Stubbed SvcSleepSystem64From32 was called."); + return SleepSystem(); } } diff --git a/mesosphere/kernel/kernel.ld b/mesosphere/kernel/kernel.ld index a42eb1b33..ae08271ac 100644 --- a/mesosphere/kernel/kernel.ld +++ b/mesosphere/kernel/kernel.ld @@ -51,7 +51,6 @@ SECTIONS . = ALIGN(8); } :code - /* .vectors. */ . = ALIGN(2K); __vectors_start__ = . ; @@ -61,6 +60,15 @@ SECTIONS . = ALIGN(8); } :code + /* .sleep. */ + . = ALIGN(4K); + __sleep_start__ = . ; + .sleep : + { + KEEP( *(.sleep .sleep.*) ) + . = ALIGN(8); + } :code + /* =========== RODATA section =========== */ . = ALIGN(0x1000); __rodata_start = . ;