diff --git a/exosphere2/program/source/secmon_cache.inc b/exosphere2/program/source/secmon_cache.inc index ea57c0f4e..8ae241ae4 100644 --- a/exosphere2/program/source/secmon_cache.inc +++ b/exosphere2/program/source/secmon_cache.inc @@ -15,6 +15,7 @@ */ void FlushEntireDataCache(); +void FlushEntireDataCacheLocal(); void InvalidateEntireDataCache(); void EnsureMappingConsistency(); diff --git a/exosphere2/program/source/secmon_cache_impl.inc b/exosphere2/program/source/secmon_cache_impl.inc index 6ef2fe4bc..a305ce698 100644 --- a/exosphere2/program/source/secmon_cache_impl.inc +++ b/exosphere2/program/source/secmon_cache_impl.inc @@ -59,6 +59,40 @@ namespace { } } + void FlushDataCacheFrom(int loc) { + for (int level = loc - 1; level >= 0; --level) { + /* Set the selection register. */ + { + util::BitPack32 csselr = {}; + csselr.Set(0); + csselr.Set(level); + HW_CPU_SET_CSSELR_EL1(csselr); + } + + /* Ensure that reordering doesn't occur around this operation. */ + hw::InstructionSynchronizationBarrier(); + + /* Get ccsidr. */ + util::BitPack32 ccsidr; + HW_CPU_GET_CCSIDR_EL1(ccsidr); + + /* Get cache size id info. */ + const int num_sets = ccsidr.Get() + 1; + const int num_ways = ccsidr.Get() + 1; + const int line_size = ccsidr.Get() + 4; + + const int way_shift = 32 - FloorLog2(num_ways); + const int set_shift = line_size; + + for (int way = 0; way <= num_ways; way++) { + for (int set = 0; set <= num_sets; set++) { + const u64 value = (static_cast(way) << way_shift) | (static_cast(set) << set_shift) | (static_cast(level) << 1); + __asm__ __volatile__("dc cisw, %[value]" :: [value]"r"(value) : "memory"); + } + } + } + } + void InvalidateDataCacheTo(int loc) { for (int level = 0; level < loc; ++level) { /* Set the selection register. */ @@ -101,6 +135,12 @@ void FlushEntireDataCache() { FlushDataCacheTo(clidr.Get()); } +void FlushEntireDataCacheLocal() { + util::BitPack32 clidr; + HW_CPU_GET_CLIDR_EL1(clidr); + FlushDataCacheFrom(clidr.Get()); +} + void InvalidateEntireDataCache() { util::BitPack32 clidr; HW_CPU_GET_CLIDR_EL1(clidr); diff --git a/exosphere2/program/source/secmon_start_virtual.s b/exosphere2/program/source/secmon_start_virtual.s index 8d23a66df..9a7a03488 100644 --- a/exosphere2/program/source/secmon_start_virtual.s +++ b/exosphere2/program/source/secmon_start_virtual.s @@ -191,6 +191,20 @@ _ZN3ams6secmon26ReleaseCommonWarmbootStackEv: /* Return. */ ret +.section .text._ZN3ams6secmon19PivotStackAndInvokeEPvPFvvE, "ax", %progbits +.align 4 +.global _ZN3ams6secmon19PivotStackAndInvokeEPvPFvvE +_ZN3ams6secmon19PivotStackAndInvokeEPvPFvvE: + /* Pivot to use the provided stack pointer. */ + mov sp, x0 + + /* Release our lock on the common smc stack. */ + mov x19, x1 + bl _ZN3ams6secmon25ReleaseCommonSmcStackLockEv + + /* Invoke the function with the new stack. */ + br x19 + .section .data._ZN3ams6secmon18CommonSmcStackLockE, "aw", %progbits .global _ZN3ams6secmon18CommonSmcStackLockE _ZN3ams6secmon18CommonSmcStackLockE: diff --git a/exosphere2/program/source/smc/secmon_smc_cpu_asm.s b/exosphere2/program/source/smc/secmon_smc_cpu_asm.s new file mode 100644 index 000000000..dd9f4dfc8 --- /dev/null +++ b/exosphere2/program/source/smc/secmon_smc_cpu_asm.s @@ -0,0 +1,94 @@ +/* + * Copyright (c) 2018-2020 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * 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 + +.section .text._ZN3ams6secmon3smc19PivotStackAndInvokeEPvPFvvE, "ax", %progbits +.align 4 +.global _ZN3ams6secmon3smc19PivotStackAndInvokeEPvPFvvE +_ZN3ams6secmon3smc19PivotStackAndInvokeEPvPFvvE: + /* Pivot to use the provided stack pointer. */ + mov sp, x0 + + /* Release our lock on the common smc stack. */ + mov x19, x1 + bl _ZN3ams6secmon25ReleaseCommonSmcStackLockEv + + /* Invoke the function with the new stack. */ + br x19 + +.section .text._ZN3ams6secmon3smc16FinalizePowerOffEv, "ax", %progbits +.align 4 +.global _ZN3ams6secmon3smc16FinalizePowerOffEv +_ZN3ams6secmon3smc16FinalizePowerOffEv: + /* Disable all caches by clearing sctlr_el1.C. */ + mrs x0, sctlr_el1 + and x0, x0, #~(1 << 2) + msr sctlr_el1, x0 + isb + + /* Disable all caches by clearing sctlr_el3.C. */ + mrs x0, sctlr_el3 + and x0, x0, #~(1 << 2) + msr sctlr_el3, x0 + isb + + /* Disable prefetching of page table walking descriptors. */ + mrs x0, cpuectlr_el1 + orr x0, x0, #(1 << 38) + + /* Disable prefetching of instructions. */ + and x0, x0, #~(3 << 35) + + /* Disable prefetching of data. */ + and x0, x0, #~(3 << 32) + msr cpuectlr_el1, x0 + isb + + /* Ensure that all data prefetching prior to our configuration change completes. */ + dsb sy + + /* Flush the entire data cache (local). */ + bl _ZN3ams6secmon25FlushEntireDataCacheLocalEv + + /* Disable receiving instruction cache/TLB maintenance operations. */ + mrs x0, cpuectlr_el1 + and x0, x0, #~(1 << 6) + msr cpuectlr_el1, x0 + + /* Configure the gic to not send interrupts to the current core. */ + ldr x1, =0x1F0043000 + mov w0, #0x1E0 /* Set FIQBypDisGrp1, IRQBypDisGrp1, reserved bits 7/8. */ + str w0, [x1] + + /* Lock the OS Double Lock. */ + mrs x0, osdlr_el1 + orr x0, x0, #(1 << 0) + msr osdlr_el1, x0 + + /* Ensure that our configuration takes. */ + isb + dsb sy + + /* Wait for interrupts, infinitely. */ +0: + wfi + b 0b + + + diff --git a/exosphere2/program/source/smc/secmon_smc_handler.cpp b/exosphere2/program/source/smc/secmon_smc_handler.cpp index c2577806c..bc044b021 100644 --- a/exosphere2/program/source/smc/secmon_smc_handler.cpp +++ b/exosphere2/program/source/smc/secmon_smc_handler.cpp @@ -248,7 +248,6 @@ namespace ams::secmon::smc { return; } - const int ind = current - LogMin; const int ofs = (ind * sizeof(args)) % LogBufSize; @@ -275,6 +274,16 @@ namespace ams::secmon::smc { DebugLog(args); } + if (args.r[0] == 0xC4000001) { + *(volatile u32 *)(MemoryRegionVirtualDebug.GetAddress()) = 0xFFFFFFFF; + *(volatile u32 *)(MemoryRegionVirtualDebug.GetAddress() + 0x10) = static_cast(hw::GetCurrentCoreId()); + + *(volatile u32 *)(MemoryRegionVirtualDevicePmc.GetAddress() + 0x50) = 0x02; + *(volatile u32 *)(MemoryRegionVirtualDevicePmc.GetAddress() + 0x00) = 0x10; + + util::WaitMicroSeconds(1000); + } + /* Get the handler info. */ const auto &info = GetHandlerInfo(table, args.r[0]); diff --git a/exosphere2/program/source/smc/secmon_smc_info.cpp b/exosphere2/program/source/smc/secmon_smc_info.cpp index c94bd7d1d..c36ef37b2 100644 --- a/exosphere2/program/source/smc/secmon_smc_info.cpp +++ b/exosphere2/program/source/smc/secmon_smc_info.cpp @@ -268,6 +268,25 @@ namespace ams::secmon::smc { return SmcResult::Success; } + SmcResult SetConfig(SmcArguments &args) { + switch (static_cast(args.r[1])) { + case ConfigItem::IsChargerHiZModeEnabled: + /* Configure the HiZ mode. */ + SetChargerHiZModeEnabled(static_cast(args.r[3])); + break; + case ConfigItem::ExosphereNeedsReboot: + /* TODO */ + return SmcResult::NotImplemented; + case ConfigItem::ExosphereNeedsShutdown: + /* TODO */ + return SmcResult::NotImplemented; + default: + return SmcResult::InvalidArgument; + } + + return SmcResult::Success; + } + } SmcResult SmcGetConfigUser(SmcArguments &args) { @@ -279,8 +298,7 @@ namespace ams::secmon::smc { } SmcResult SmcSetConfig(SmcArguments &args) { - /* TODO */ - return SmcResult::NotImplemented; + return SetConfig(args); } /* This is an atmosphere extension smc. */ diff --git a/exosphere2/program/source/smc/secmon_smc_power_management.cpp b/exosphere2/program/source/smc/secmon_smc_power_management.cpp index 92cbd3491..3af573a75 100644 --- a/exosphere2/program/source/smc/secmon_smc_power_management.cpp +++ b/exosphere2/program/source/smc/secmon_smc_power_management.cpp @@ -14,12 +14,24 @@ * along with this program. If not, see . */ #include +#include "../secmon_cache.hpp" #include "../secmon_cpu_context.hpp" #include "../secmon_error.hpp" #include "secmon_smc_power_management.hpp" +namespace ams::secmon { + + /* Declare assembly functionality. */ + void *GetCoreExceptionStackVirtual(); + +} + namespace ams::secmon::smc { + /* Declare assembly power-management functionality. */ + void PivotStackAndInvoke(void *stack, void (*function)()); + void FinalizePowerOff(); + namespace { constexpr inline uintptr_t PMC = MemoryRegionVirtualDevicePmc.GetAddress(); @@ -94,11 +106,44 @@ namespace ams::secmon::smc { REG_BITS_VALUE(which_core + 0x10, 1, 1)); /* CORERESETn */ } + void PowerOffCpu() { + /* Get the current core id. */ + const auto core_id = hw::GetCurrentCoreId(); + + /* Configure the flow controller to prepare for shutting down the current core. */ + flow::SetCpuCsr(core_id, FLOW_CTLR_CPUN_CSR_ENABLE_EXT_DISABLE); + flow::SetHaltCpuEvents(core_id, false); + flow::SetCc4Ctrl(core_id, 0); + + /* Save the core's context for restoration on next power-on. */ + SaveDebugRegisters(); + SetCoreOff(); + + /* Ensure there are no pending memory transactions prior to our power-down. */ + FlushEntireDataCache(); + + /* Finalize our powerdown and wait for an interrupt. */ + FinalizePowerOff(); + } + } SmcResult SmcPowerOffCpu(SmcArguments &args) { - /* TODO */ - return SmcResult::NotImplemented; + /* Get the current core id. */ + const auto core_id = hw::GetCurrentCoreId(); + + /* Note that we're expecting a reset for the current core. */ + SetResetExpected(true); + + /* If we're on the final core, shut down directly. Otherwise, invoke with special stack. */ + if (core_id == NumCores - 1) { + PowerOffCpu(); + } else { + PivotStackAndInvoke(GetCoreExceptionStackVirtual(), PowerOffCpu); + } + + /* This code will never be reached. */ + __builtin_unreachable(); } SmcResult SmcPowerOnCpu(SmcArguments &args) { diff --git a/libraries/libexosphere/include/exosphere/flow.hpp b/libraries/libexosphere/include/exosphere/flow.hpp index 93cfb98c3..210ecceda 100644 --- a/libraries/libexosphere/include/exosphere/flow.hpp +++ b/libraries/libexosphere/include/exosphere/flow.hpp @@ -22,4 +22,8 @@ namespace ams::flow { void ResetCpuRegisters(int core); + void SetCpuCsr(int core, u32 enable_ext); + void SetHaltCpuEvents(int core, bool resume_on_irq); + void SetCc4Ctrl(int core, u32 value); + } diff --git a/libraries/libexosphere/include/exosphere/tegra/tegra_flow_ctlr.hpp b/libraries/libexosphere/include/exosphere/tegra/tegra_flow_ctlr.hpp index d30493330..7c2f78919 100644 --- a/libraries/libexosphere/include/exosphere/tegra/tegra_flow_ctlr.hpp +++ b/libraries/libexosphere/include/exosphere/tegra/tegra_flow_ctlr.hpp @@ -47,6 +47,19 @@ #define DEFINE_FLOW_REG_THREE_BIT_ENUM(NAME, __OFFSET__, ZERO, ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN) REG_DEFINE_NAMED_THREE_BIT_ENUM(FLOW_CTLR, NAME, __OFFSET__, ZERO, ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN) #define DEFINE_FLOW_REG_FOUR_BIT_ENUM(NAME, __OFFSET__, ZERO, ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN, EIGHT, NINE, TEN, ELEVEN, TWELVE, THIRTEEN, FOURTEEN, FIFTEEN) REG_DEFINE_NAMED_FOUR_BIT_ENUM (FLOW_CTLR, NAME, __OFFSET__, ZERO, ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN, EIGHT, NINE, TEN, ELEVEN, TWELVE, THIRTEEN, FOURTEEN, FIFTEEN) +DEFINE_FLOW_REG_BIT_ENUM(CPUN_CSR_ENABLE, 0, DISABLE, ENABLE); +DEFINE_FLOW_REG(CPUN_CSR_WAIT_WFI_BITMAP, 8, 4); +DEFINE_FLOW_REG_BIT_ENUM(CPUN_CSR_ENABLE_EXT, 12, DISABLE, ENABLE); +DEFINE_FLOW_REG_BIT_ENUM(CPUN_CSR_EVENT_FLAG, 14, FALSE, TRUE); +DEFINE_FLOW_REG_BIT_ENUM(CPUN_CSR_INTR_FLAG, 15, FALSE, TRUE); + +DEFINE_FLOW_REG_BIT_ENUM(HALT_CPUN_EVENTS_GIC_FIQN, 8, DISABLE, ENABLE); +DEFINE_FLOW_REG_BIT_ENUM(HALT_CPUN_EVENTS_GIC_IRQN, 9, DISABLE, ENABLE); +DEFINE_FLOW_REG_BIT_ENUM(HALT_CPUN_EVENTS_LIC_FIQN, 10, DISABLE, ENABLE); +DEFINE_FLOW_REG_BIT_ENUM(HALT_CPUN_EVENTS_LIC_IRQN, 11, DISABLE, ENABLE); + +DEFINE_FLOW_REG_THREE_BIT_ENUM(HALT_CPUN_EVENTS_FLOW_MODE, 29, NONE, RUN_AND_INT, WAITEVENT, WAITEVENT_AND_INT, STOP_UNTIL_IRQ, STOP_UNTIL_EVENT_AND_IRQ, RESERVED6, RESERVED7); + DEFINE_FLOW_REG_BIT_ENUM(HALT_COP_EVENTS_JTAG, 28, ENABLED, DISABLED); DEFINE_FLOW_REG_THREE_BIT_ENUM(HALT_COP_EVENTS_MODE, 29, FLOW_MODE_NONE, FLOW_MODE_RUN_AND_INT, FLOW_MODE_STOP, FLOW_MODE_STOP_AND_INT, FLOW_MODE_STOP_UNTIL_IRQ, FLOW_MODE_STOP_UNTIL_IRQ_AND_INT, FLOW_MODE_STOP_UNTIL_EVENT_AND_IRQ, RESERVED7); diff --git a/libraries/libexosphere/source/flow/flow_api.cpp b/libraries/libexosphere/source/flow/flow_api.cpp index 613c78c07..82a19943b 100644 --- a/libraries/libexosphere/source/flow/flow_api.cpp +++ b/libraries/libexosphere/source/flow/flow_api.cpp @@ -34,6 +34,18 @@ namespace ams::flow { { FLOW_CTLR_CPU3_CSR, FLOW_CTLR_HALT_CPU3_EVENTS, FLOW_CTLR_CC4_CORE3_CTRL, }, }; + constexpr u32 GetHaltCpuEventsValue(bool resume_on_irq) { + if (resume_on_irq) { + return reg::Encode(FLOW_REG_BITS_ENUM(HALT_CPUN_EVENTS_FLOW_MODE, WAITEVENT), + FLOW_REG_BITS_ENUM(HALT_CPUN_EVENTS_LIC_IRQN, ENABLE), + FLOW_REG_BITS_ENUM(HALT_CPUN_EVENTS_LIC_FIQN, ENABLE), + FLOW_REG_BITS_ENUM(HALT_CPUN_EVENTS_GIC_IRQN, ENABLE), + FLOW_REG_BITS_ENUM(HALT_CPUN_EVENTS_GIC_FIQN, ENABLE)); + } else { + return reg::Encode(FLOW_REG_BITS_ENUM(HALT_CPUN_EVENTS_FLOW_MODE, WAITEVENT)); + } + } + } void SetRegisterAddress(uintptr_t address) { @@ -48,4 +60,20 @@ namespace ams::flow { reg::Write(g_register_address + offsets.halt_cpu_events, 0); } -} \ No newline at end of file + void SetCpuCsr(int core, u32 enable_ext) { + reg::Write(g_register_address + FlowControllerRegisterOffsets[core].cpu_csr, FLOW_REG_BITS_ENUM (CPUN_CSR_INTR_FLAG, TRUE), + FLOW_REG_BITS_ENUM (CPUN_CSR_EVENT_FLAG, TRUE), + FLOW_REG_BITS_VALUE(CPUN_CSR_ENABLE_EXT, enable_ext), + FLOW_REG_BITS_VALUE(CPUN_CSR_WAIT_WFI_BITMAP, (1u << core)), + FLOW_REG_BITS_ENUM (CPUN_CSR_ENABLE, ENABLE)); + } + + void SetHaltCpuEvents(int core, bool resume_on_irq) { + reg::Write(g_register_address + FlowControllerRegisterOffsets[core].halt_cpu_events, GetHaltCpuEventsValue(resume_on_irq)); + } + + void SetCc4Ctrl(int core, u32 value) { + reg::Write(g_register_address + FlowControllerRegisterOffsets[core].cc4_core_ctrl, value); + } + +}