exo2: implement smc cpu off

exosphere2/program/source/secmon_cache.inc

@@ -15,6 +15,7 @@
  */
 
 void FlushEntireDataCache();
+void FlushEntireDataCacheLocal();
 void InvalidateEntireDataCache();
 
 void EnsureMappingConsistency();

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<hw::CsselrEl1::InD>(0);
+                csselr.Set<hw::CsselrEl1::Level>(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<hw::CcsidrEl1::NumSets>() + 1;
+            const int num_ways  = ccsidr.Get<hw::CcsidrEl1::Associativity>() + 1;
+            const int line_size = ccsidr.Get<hw::CcsidrEl1::LineSize>() + 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<u64>(way) << way_shift) | (static_cast<u64>(set) << set_shift) | (static_cast<u64>(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<hw::ClidrEl1::Loc>());
 }
 
+void FlushEntireDataCacheLocal() {
+    util::BitPack32 clidr;
+    HW_CPU_GET_CLIDR_EL1(clidr);
+    FlushDataCacheFrom(clidr.Get<hw::ClidrEl1::Louis>());
+}
+
 void InvalidateEntireDataCache() {
     util::BitPack32 clidr;
     HW_CPU_GET_CLIDR_EL1(clidr);

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:

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 <http://www.gnu.org/licenses/>.
+ */
+
+/* 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
+
+
+

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<u32>(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]);
 

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<ConfigItem>(args.r[1])) {
+                case ConfigItem::IsChargerHiZModeEnabled:
+                    /* Configure the HiZ mode. */
+                    SetChargerHiZModeEnabled(static_cast<bool>(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 SetConfig(args);
-        return SmcResult::NotImplemented;
     }
 
     /* This is an atmosphere extension smc. */

exosphere2/program/source/smc/secmon_smc_power_management.cpp

@@ -14,12 +14,24 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <exosphere.hpp>
+#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 */
+        /* Get the current core id. */
-        return SmcResult::NotImplemented;
+        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) {

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);
+
 }

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);
 

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);
     }
 
-}
+    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);
+    }
+
+}