exo2: implement the first half of SmcCpuSuspend

exosphere2/program/source/smc/secmon_smc_power_management.cpp

@@ -18,6 +18,7 @@
 #include "../secmon_cpu_context.hpp"
 #include "../secmon_error.hpp"
 #include "secmon_smc_power_management.hpp"
+#include "secmon_smc_se_lock.hpp"
 
 namespace ams::secmon {
 
@@ -35,8 +36,27 @@ namespace ams::secmon::smc {
     namespace {
 
         constexpr inline uintptr_t PMC     = MemoryRegionVirtualDevicePmc.GetAddress();
+        constexpr inline uintptr_t GPIO    = MemoryRegionVirtualDeviceGpio.GetAddress();
         constexpr inline uintptr_t CLK_RST = MemoryRegionVirtualDeviceClkRst.GetAddress();
 
+        constexpr inline uintptr_t CommonSmcStackTop = MemoryRegionVirtualTzramVolatileData.GetEndAddress() - (0x80 * (NumCores - 1));
+
+        enum PowerStateType {
+            PowerStateType_StandBy   = 0,
+            PowerStateType_PowerDown = 1,
+        };
+
+        enum PowerStateId {
+            PowerStateId_Sc7 = 27,
+        };
+
+        /* http://infocenter.arm.com/help/topic/com.arm.doc.den0022d/Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf Page 46 */
+        struct SuspendCpuPowerState {
+            using StateId    = util::BitPack32::Field< 0, 16, PowerStateId>;
+            using StateType  = util::BitPack32::Field<16,  1, PowerStateType>;
+            using PowerLevel = util::BitPack32::Field<24,  2, u32>;
+        };
+
         constinit bool g_charger_hi_z_mode_enabled = false;
 
         constinit const reg::BitsMask CpuPowerGateStatusMasks[NumCores] = {
@@ -126,6 +146,95 @@ namespace ams::secmon::smc {
             FinalizePowerOff();
         }
 
+        void ValidateSocStateForSuspend() {
+            /* TODO */
+        }
+
+        void SaveSecureContextAndSuspend() {
+            /* TODO */
+
+            /* Finalize our powerdown and wait for an interrupt. */
+            FinalizePowerOff();
+        }
+
+        SmcResult SuspendCpuImpl(SmcArguments &args) {
+            /* Decode arguments. */
+            const util::BitPack32 power_state = { static_cast<u32>(args.r[1]) };
+            const uintptr_t       entry_point = args.r[2];
+            const uintptr_t       context_id  = args.r[3];
+
+            const auto state_type = power_state.Get<SuspendCpuPowerState::StateType>();
+            const auto state_id   = power_state.Get<SuspendCpuPowerState::StateId>();
+
+            const auto core_id = hw::GetCurrentCoreId();
+
+            /* Validate arguments. */
+            SMC_R_UNLESS(state_type == PowerStateType_PowerDown, PsciDenied);
+            SMC_R_UNLESS(state_id   == PowerStateId_Sc7,         PsciDenied);
+
+            /* Orchestrate charger transition to Hi-Z mode if needed. */
+            if (IsChargerHiZModeEnabled()) {
+                /* Ensure we can do comms over i2c-1. */
+                clkrst::EnableI2c1Clock();
+
+                /* If the charger isn't in hi-z mode, perform a transition. */
+                if (!charger::IsHiZMode()) {
+                    charger::EnterHiZMode();
+
+                    /* Wait up to 50ms for the transition to complete. */
+                    const auto start_time = util::GetMicroSeconds();
+                    auto current_time = start_time;
+                    while ((current_time - start_time) <= 50'000) {
+                        if (auto intr_status = reg::Read(GPIO + 0x634); (intr_status & 1) == 0) {
+                            /* Wait 256 us to ensure the transition completes. */
+                            util::WaitMicroSeconds(256);
+                            break;
+                        }
+                        current_time = util::GetMicroSeconds();
+                    }
+                }
+
+                /* Disable i2c-1, since we're done communicating over it. */
+                clkrst::DisableI2c1Clock();
+            }
+
+            /* Enable wake event detection. */
+            pmc::EnableWakeEventDetection();
+
+            /* Ensure that i2c-5 is usable for communicating with the pmic. */
+            clkrst::EnableI2c5Clock();
+            i2c::Initialize(i2c::Port_5);
+
+            /* Orchestrate sleep entry with the pmic. */
+            pmic::EnableSleep();
+
+            /* Ensure that the soc is in a state valid for us to suspend. */
+            ValidateSocStateForSuspend();
+
+            /* Configure the pmc for sc7 entry. */
+            pmc::ConfigureForSc7Entry();
+
+            /* Configure the flow controller for sc7 entry. */
+            flow::SetCc4Ctrl(core_id, 0);
+            flow::SetHaltCpuEvents(core_id, false);
+            flow::ClearL2FlushControl();
+            flow::SetCpuCsr(core_id, FLOW_CTLR_CPUN_CSR_ENABLE_EXT_POWERGATE_CPU_TURNOFF_CPURAIL);
+
+            /* Save the entry context. */
+            SetEntryContext(core_id, entry_point, context_id);
+
+            /* Configure the cpu context for reset. */
+            SaveDebugRegisters();
+            SetCoreOff();
+            SetResetExpected(true);
+
+            /* Switch to use the common smc stack (all other cores are off), and perform suspension. */
+            PivotStackAndInvoke(reinterpret_cast<void *>(CommonSmcStackTop), SaveSecureContextAndSuspend);
+
+            /* This code will never be reached. */
+            __builtin_unreachable();
+        }
+
     }
 
     SmcResult SmcPowerOffCpu(SmcArguments &args) {
@@ -170,8 +279,7 @@ namespace ams::secmon::smc {
     }
 
     SmcResult SmcSuspendCpu(SmcArguments &args) {
-        /* TODO */
+        return LockSecurityEngineAndInvoke(args, SuspendCpuImpl);
-        return SmcResult::NotImplemented;
     }
 
     bool IsChargerHiZModeEnabled() {

libraries/libexosphere/include/exosphere/clkrst.hpp

@@ -27,6 +27,7 @@ namespace ams::clkrst {
     void EnableUartCClock();
     void EnableActmonClock();
     void EnableI2c1Clock();
+    void EnableI2c5Clock();
 
     void DisableI2c1Clock();
 

libraries/libexosphere/include/exosphere/flow.hpp

@@ -25,5 +25,6 @@ namespace ams::flow {
     void SetCpuCsr(int core, u32 enable_ext);
     void SetHaltCpuEvents(int core, bool resume_on_irq);
     void SetCc4Ctrl(int core, u32 value);
+    void ClearL2FlushControl();
 
 }

libraries/libexosphere/include/exosphere/pmc.hpp

@@ -36,6 +36,8 @@ namespace ams::pmc {
     void SetRegisterAddress(uintptr_t address);
 
     void InitializeRandomScratch();
+    void EnableWakeEventDetection();
+    void ConfigureForSc7Entry();
 
     void LockSecureRegister(SecureRegister reg);
 

libraries/libexosphere/include/exosphere/pmic.hpp

@@ -29,5 +29,7 @@ namespace ams::pmic {
 
     void EnableVddCpu(Regulator regulator);
     void DisableVddCpu(Regulator regulator);
+    void EnableSleep();
+    bool IsAcOk();
 
 }

libraries/libexosphere/include/exosphere/tegra/tegra_flow_ctlr.hpp

@@ -18,6 +18,7 @@
 
 
 #define FLOW_CTLR_FLOW_DBG_QUAL        (0x050)
+#define FLOW_CTLR_L2FLUSH_CONTROL      (0x094)
 #define FLOW_CTLR_BPMP_CLUSTER_CONTROL (0x098)
 
 #define FLOW_CTLR_CPU0_CSR             (0x008)

libraries/libexosphere/include/exosphere/tegra/tegra_pmc.hpp

@@ -212,3 +212,5 @@ DEFINE_PMC_REG_BIT_ENUM(CLAMP_STATUS_XUSBB, 21, DISABLE, ENABLE);
 DEFINE_PMC_REG_BIT_ENUM(CLAMP_STATUS_XUSBC, 22, DISABLE, ENABLE);
 DEFINE_PMC_REG_BIT_ENUM(CLAMP_STATUS_VIC,   23, DISABLE, ENABLE);
 DEFINE_PMC_REG_BIT_ENUM(CLAMP_STATUS_IRAM,  24, DISABLE, ENABLE);
+
+DEFINE_PMC_REG_BIT_ENUM(CNTRL2_WAKE_DET_EN, 9, DISABLE, ENABLE);

libraries/libexosphere/source/clkrst/clkrst_api.cpp

@@ -105,7 +105,7 @@ namespace ams::clkrst {
     }
 
     void EnableI2c5Clock() {
-        EnableClock(I2c1Clock);
+        EnableClock(I2c5Clock);
     }
 
     void DisableI2c1Clock() {

libraries/libexosphere/source/flow/flow_api.cpp

@@ -76,4 +76,8 @@ namespace ams::flow {
         reg::Write(g_register_address + FlowControllerRegisterOffsets[core].cc4_core_ctrl, value);
     }
 
+    void ClearL2FlushControl() {
+        reg::Write(g_register_address + FLOW_CTLR_L2FLUSH_CONTROL, 0);
+    }
+
 }

libraries/libexosphere/source/pmc/pmc_api.cpp

@@ -224,6 +224,32 @@ namespace ams::pmc {
         LockSecureRegister(SecureRegister_Srk);
     }
 
+    void EnableWakeEventDetection() {
+        /* Get the address. */
+        const uintptr_t address = g_register_address;
+
+        /* Wait 75us, then enable event detection, then wait another 75us. */
+        util::WaitMicroSeconds(75);
+        reg::ReadWrite(address + APBDEV_PMC_CNTRL2,  PMC_REG_BITS_ENUM(CNTRL2_WAKE_DET_EN, ENABLE));
+        util::WaitMicroSeconds(75);
+
+        /* Enable all wake events. */
+        reg::Write(address + APBDEV_PMC_WAKE_STATUS,  0xFFFFFFFFu);
+        reg::Write(address + APBDEV_PMC_WAKE2_STATUS, 0xFFFFFFFFu);
+        util::WaitMicroSeconds(75);
+    }
+
+    void ConfigureForSc7Entry() {
+        /* Get the address. */
+        const uintptr_t address = g_register_address;
+
+        /* Configure the bootrom to perform a warmboot. */
+        reg::Write(address + APBDEV_PMC_SCRATCH0, 0x1);
+
+        /* Enable the TSC multiplier. */
+        reg::ReadWrite(address + APBDEV_PMC_DPD_ENABLE, PMC_REG_BITS_ENUM(DPD_ENABLE_TSC_MULT_EN, ENABLE));
+    }
+
     void LockSecureRegister(SecureRegister reg) {
         /* Get the address. */
         const uintptr_t address = g_register_address;

libraries/libexosphere/source/pmic/pmic_api.cpp

@@ -25,13 +25,17 @@ namespace ams::pmic {
         constexpr inline int I2cAddressMarikoMax77812_A = 0x31;
         constexpr inline int I2cAddressMarikoMax77812_B = 0x33;
 
+        constexpr inline int I2cAddressMax77620Pmic     = 0x3C;
 
+        /* https://github.com/Atmosphere-NX/Atmosphere/blob/master/emummc/source/power/max77620.h */
         /* https://github.com/Atmosphere-NX/Atmosphere/blob/master/emummc/source/power/max7762x.h */
         /* TODO: Find datasheet, link to it instead. */
         /* NOTE: Tentatively, Max77620 "mostly" matches https://datasheets.maximintegrated.com/en/ds/MAX77863.pdf. */
         /*       This does not contain Max77621 documentation, though. */
+        constexpr inline int Max77620RegisterOnOffStat  = 0x15;
         constexpr inline int Max77620RegisterGpio0      = 0x36;
         constexpr inline int Max77620RegisterAmeGpio    = 0x40;
+        constexpr inline int Max77620RegisterOnOffCnfg1 = 0x41;
 
         constexpr inline int Max77621RegisterVOut     = 0x00;
         constexpr inline int Max77621RegisterVOutDvc  = 0x01;
@@ -108,6 +112,10 @@ namespace ams::pmic {
             }
         }
 
+        u8 GetPmicOnOffStat() {
+            return i2c::QueryByte(i2c::Port_5, I2cAddressMax77620Pmic, Max77620RegisterOnOffStat);
+        }
+
     }
 
     void EnableVddCpu(Regulator regulator) {
@@ -132,4 +140,19 @@ namespace ams::pmic {
         }
     }
 
+    void EnableSleep() {
+        /* Get the current onoff cfg. */
+        u8 cnfg = i2c::QueryByte(i2c::Port_5, I2cAddressMax77620Pmic, Max77620RegisterOnOffCnfg1);
+
+        /* Set SlpEn. */
+        cnfg |= (1 << 2);
+
+        /* Write the new cfg. */
+        i2c::SendByte(i2c::Port_5, I2cAddressMax77620Pmic, Max77620RegisterOnOffCnfg1, cnfg);
+    }
+
+    bool IsAcOk() {
+        return (GetPmicOnOffStat() & (1 << 1)) != 0;
+    }
+
 }