exo2: implement smc cpu off

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 SmcResult::NotImplemented;
+        return SetConfig(args);
     }
 
     /* 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 */
-        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) {