thermosphere: rewrite smc traps

thermosphere/src/cpu/hvisor_cpu_caches.cpp

@@ -102,6 +102,10 @@ namespace ams::hvisor::cpu {
         }
         if (!(ctr & BIT(29))) {
             InvalidateInstructionCacheRangePoU(addr, size);
+        } else {
+            // Make sure we have at least a dsb/isb
+            dsb();
+            isb();
         }
     }
 

thermosphere/src/exception_vectors.s

@@ -234,19 +234,30 @@ _restoreAllRegisters:
 
 UNKNOWN_EXCEPTION       _serrorSp0
 
-/* To save space, insert in an unused vector segment. */
+// To save space, insert in an unused vector segment.
 .global semihosting_call
 .type   semihosting_call, %function
 .func   semihosting_call
 .cfi_startproc
+.cfi_sections   .debug_frame
 semihosting_call:
     hlt     #0xF000
     ret
 .cfi_endproc
 .endfunc
 
-.global doSmcIndirectCallImpl
-doSmcIndirectCallImpl:
+// To save space, insert in an unused vector segment.
+
+// ams::hvisor::traps::CallSmc0(ams::hvisor::ExceptionStackFrame*):
+.global _ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE
+.type   _ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE, %function
+.func   _ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE
+.cfi_startproc
+.cfi_sections   .debug_frame
+// ams::hvisor::callSmcTemplate[]
+.global _ZN3ams6hvisor5traps15callSmcTemplateE
+_ZN3ams6hvisor5traps15callSmcTemplateE:
+_ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE:
     stp     x19, x20, [sp, #-0x10]!
     mov     x19, x0
 
@@ -255,9 +266,10 @@ doSmcIndirectCallImpl:
     ldp     x4, x5, [x19, #0x20]
     ldp     x6, x7, [x19, #0x30]
 
-    _doSmcIndirectCallImplSmcInstruction:
+    _callSmcTemplateSmcInstruction:
     smc     #0
 
+    // Note that NN's secure monitor can return results in x4-x7, this differs from Arm's spec.
     stp     x0, x1, [x19, #0x00]
     stp     x2, x3, [x19, #0x10]
     stp     x4, x5, [x19, #0x20]
@@ -266,13 +278,44 @@ doSmcIndirectCallImpl:
     ldp     x19, x20, [sp], #0x10
     ret
 
-_doSmcIndirectCallImplEnd:
-.global doSmcIndirectCallImplSmcInstructionOffset
-doSmcIndirectCallImplSmcInstructionOffset:
-    .word _doSmcIndirectCallImplSmcInstruction - doSmcIndirectCallImpl
-.global doSmcIndirectCallImplSize
-doSmcIndirectCallImplSize:
-    .word _doSmcIndirectCallImplEnd - doSmcIndirectCallImpl
+_callSmcTemplateEnd:
+.endfunc
+
+// ams::hvisor::traps::callSmcTemplateInstructionOffset
+.global _ZN3ams6hvisor5traps32callSmcTemplateInstructionOffsetE
+_ZN3ams6hvisor5traps32callSmcTemplateInstructionOffsetE:
+    .word _callSmcTemplateSmcInstruction - _ZN3ams6hvisor5traps15callSmcTemplateE
+// ams::hvisor::traps::callSmcTemplateSize
+.global _ZN3ams6hvisor5traps19callSmcTemplateSizeE
+_ZN3ams6hvisor5traps19callSmcTemplateSizeE:
+    .word _callSmcTemplateEnd - _ZN3ams6hvisor5traps15callSmcTemplateE
+
+// ams::hvisor::traps::CallSmc1(ams::hvisor::ExceptionStackFrame*):
+.global _ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE
+.type   _ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE, %function
+.func   _ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE
+.cfi_startproc
+.cfi_sections   .debug_frame
+_ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE:
+    stp     x19, x20, [sp, #-0x10]!
+    mov     x19, x0
+
+    ldp     x0, x1, [x19, #0x00]
+    ldp     x2, x3, [x19, #0x10]
+    ldp     x4, x5, [x19, #0x20]
+    ldp     x6, x7, [x19, #0x30]
+
+    smc     #1
+
+    // Note that NN's secure monitor can return results in x4-x7, this differs from Arm's spec.
+    stp     x0, x1, [x19, #0x00]
+    stp     x2, x3, [x19, #0x10]
+    stp     x4, x5, [x19, #0x20]
+    stp     x6, x7, [x19, #0x30]
+
+    ldp     x19, x20, [sp], #0x10
+    ret
+.endfunc
 
 /* Current EL, SPx */
 

thermosphere/src/hvisor_core_context.hpp

@@ -79,11 +79,13 @@ namespace ams::hvisor {
             constexpr u32 GetCoreId() const                                 { return m_coreId; }
             constexpr bool IsBootCore() const                               { return m_bootCore; }
 
-            constexpr u64 SetWarmboot(uintptr_t ep)
+            constexpr u64 SetKernelEntrypoint(uintptr_t ep, bool warmboot = false)
             {
+                if (warmboot) {
                     // No race possible, only possible transition is 1->0 and we only really check IsColdboot() at init time
                     // And CPU_SUSPEND should only be called with only one core left.
                     coldboot = false;
+                }
                 m_kernelEntrypoint = ep;
             }
 

thermosphere/src/traps/hvisor_traps_smc.cpp

@@ -0,0 +1,132 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#include "hvisor_traps_smc.hpp"
+#include "../hvisor_core_context.hpp"
+#include "../cpu/hvisor_cpu_caches.hpp"
+
+#include "../debug_manager.h"
+#include "../memory_map.h"
+
+namespace {
+
+    void CpuOnHook(ams::hvisor::ExceptionStackFrame *frame)
+    {
+        // Note: preserve contexId which is passed by EL3, we'll save it later
+        // Note: full affinity mask in x1. We assume there's only one cluster
+
+        // Note: not safe if called concurrently for the same target core
+        // But I guess sane code would call it with the same ep,ctxId values anyway...
+        u32 cpuId = frame->ReadRegister<u32>(1);
+        uintptr_t ep = frame->ReadRegister(2);
+        // x3 is contextId
+        if (cpuId < MAX_CORE) {
+            auto &ctx = ams::hvisor::CoreContext::GetInstanceFor(cpuId);
+            ctx.SetKernelEntrypoint(ep);
+            frame->WriteRegister(2, g_loadImageLayout.startPa + 4); //FIXME
+        }
+        ams::hvisor::cpu::dmb();
+    }
+
+    inline void CpuOffHook(ams::hvisor::ExceptionStackFrame *frame)
+    {
+        debugManagerReportEvent(DBGEVENT_CORE_OFF);
+        ams::hvisor::cpu::dmb();
+    }
+
+    void CpuSuspendHook(ams::hvisor::ExceptionStackFrame *frame, size_t epIdx)
+    {
+        // We may trigger warmboot, depending on powerState (x1 or default value)
+        uintptr_t ep = frame->ReadRegister(epIdx);
+        ams::hvisor::currentCoreCtx->SetKernelEntrypoint(ep, true);
+        frame->WriteRegister(epIdx, g_loadImageLayout.startPa + 4); //FIXME
+
+        ams::hvisor::cpu::dmb();
+    }
+
+}
+namespace ams::hvisor::traps {
+
+    void CallSmcIndirect(ExceptionStackFrame *frame, u32 smcId) {
+        if (AMS_LIKELY(smcId == 0)) {
+            CallSmc0(frame);
+        } else if (smcId == 1) {
+            CallSmc1(frame);
+        } else {
+            // Need to do self-modifying code here
+            // Assume num < 16 to avoid a VLA and save some instructions
+            size_t num = callSmcTemplateInstructionSize / 4;
+            u32 codebuf[16];
+
+            std::copy(callSmcTemplate, callSmcTemplate + num, codebuf);
+            codebuf[callSmcTemplateInstructionOffset / 4] |= smcId << 5;
+
+            cpu::HandleSelfModifyingCodePoU(codebuf, sizeof(codebuf));
+            reinterpret_cast<decltype(&CallSmc0)>(codebuf)(frame);
+        }
+    }
+
+    void HandleSmcTrap(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr)
+    {
+        // TODO: Arm PSCI 0.2+ stuff
+        u32 smcId = esr.iss;
+
+        // Note: don't differenciate PSCI calls by smc immediate since HOS uses #1 for that
+        // Note: funcId is actually u32 according to Arm PSCI. Not sure what to do if x0>>32 != 0.
+        // NN doesn't truncate, Arm TF does.
+        // Note: clear NN ABI-breaking bits
+        u32 funcId = frame->x[0] & ~0xFF00;
+
+        // Hooks go here
+        switch (funcId) {
+            case 0xC4000001: {
+                // CPU_SUSPEND
+                CpuSuspendHook(frame, 2);
+                break;
+            }
+            case 0xC400000C:
+            case 0xC400000E: {
+                // CPU_DEFAULT_SUSPEND
+                // SYSTEM_SUSPEND
+                // (neither is implemented in NN's secure monitor...)
+                CpuSuspendHook(frame, 1);
+                break;
+            }
+            case 0x84000002: {
+                // CPU_OFF
+                CpuOffHook(frame);
+                break;
+            }
+            case 0xC4000003: {
+                // CPU_ON
+                CpuOnHook(frame);
+                break;
+            }
+            case 0x84000008:
+            case 0x84000009:
+                // SYSTEM_OFF, not implemented by Nintendo & we dont't support it in our debugger model (yet?)
+                // SYSTEM_RESET, same
+                break;
+
+            default:
+                // Other unimportant functions we don't care about
+                break;
+        }
+        CallSmcIndirect(frame, smcId);
+        frame->SkipInstruction(4);
+    }
+
+}

thermosphere/src/traps/hvisor_traps_smc.hpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 Atmosphère-NX
+ * Copyright (c) 2019-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,
@@ -16,8 +16,18 @@
 
 #pragma once
 
-#include "traps.h"
+#include "../hvisor_exception_stack_frame.hpp"
 
-void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId);
+namespace ams::hvisor::traps {
 
-void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+    // In exception_vectors.s:
+    extern const u32 callSmcTemplate[];
+    extern const u32 callSmcTemplateInstructionOffset;
+    extern const u32 callSmcTemplateInstructionSize;
+    void CallSmc0(ExceptionStackFrame *frame);
+    void CallSmc1(ExceptionStackFrame *frame);
+
+    void CallSmcIndirect(ExceptionStackFrame *frame, u32 smcId);
+    void HandleSmc(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
+
+}

thermosphere/src/traps/smc.c

@@ -1,75 +0,0 @@
-#include <string.h>
-#include "smc.h"
-#include "core_ctx.h"
-#include "caches.h"
-#include "memory_map.h"
-#include "debug_manager.h"
-
-// Currently in exception_vectors.s:
-extern const u32 doSmcIndirectCallImpl[];
-extern const u32 doSmcIndirectCallImplSmcInstructionOffset, doSmcIndirectCallImplSize;
-
-void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId)
-{
-    u32 codebuf[doSmcIndirectCallImplSize / 4]; // note: potential VLA
-    memcpy(codebuf, doSmcIndirectCallImpl, doSmcIndirectCallImplSize);
-    codebuf[doSmcIndirectCallImplSmcInstructionOffset / 4] |= smcId << 5;
-
-    cacheHandleSelfModifyingCodePoU(codebuf, doSmcIndirectCallImplSize/4);
-
-    __dsb();
-    __isb();
-    ((void (*)(ExceptionStackFrame *))codebuf)(frame);
-}
-
-static void doCpuOnHook(ExceptionStackFrame *frame, u32 smcId)
-{
-    // Note: preserve contexId which is passed by EL3, we'll save it later
-    u32 cpuId = (u32)frame->x[1]; // note: full affinity mask. Start.s checks that Aff1,2,3 are 0.
-    uintptr_t ep = frame->x[2];
-    // frame->x[3] is contextId
-    if (cpuId < 4) {
-        g_coreCtxs[cpuId].kernelEntrypoint = ep;
-        frame->x[2] = g_loadImageLayout.startPa + 4;
-    }
-}
-
-void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
-{
-    // TODO: Arm PSCI 0.2+ stuff
-    u32 smcId = esr.iss;
-
-    // Note: don't differenciate PSCI calls by smc immediate since HOS uses #1 for that
-    // Note: funcId is actually u32 according to Arm PSCI. Not sure what to do if x0>>32 != 0.
-    // NN doesn't truncate, Arm TF does.
-    // Note: clear NN ABI-breaking bits
-    u32 funcId = frame->x[0] & ~0xFF00;
-    switch (funcId) {
-        case 0xC4000001:
-            // CPU_SUSPEND
-            // TODO
-            break;
-        case 0x84000002:
-            // CPU_OFF
-            // TODO
-            debugManagerReportEvent(DBGEVENT_CORE_OFF);
-            break;
-        case 0xC4000003:
-            doCpuOnHook(frame, smcId);
-            break;
-        case 0x84000008:
-            // SYSTEM_OFF, not implemented by Nintendo
-            // TODO
-            break;
-        case 0x84000009:
-            // SYSTEM_RESET, not implemented by Nintendo
-            // TODO
-            break;
-
-        default:
-            // Other unimportant functions we don't care about
-            break;
-    }
-    doSmcIndirectCall(frame, smcId);
-    skipFaultingInstruction(frame, 4);
-}