diff --git a/thermosphere/src/cpu/hvisor_cpu_caches.cpp b/thermosphere/src/cpu/hvisor_cpu_caches.cpp
index 2889d9a9a..b38418699 100644
--- a/thermosphere/src/cpu/hvisor_cpu_caches.cpp
+++ b/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();
}
}
diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s
index 618c73b8b..ef1b76d80 100644
--- a/thermosphere/src/exception_vectors.s
+++ b/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 */
diff --git a/thermosphere/src/hvisor_core_context.hpp b/thermosphere/src/hvisor_core_context.hpp
index f183a3236..4adc2c334 100644
--- a/thermosphere/src/hvisor_core_context.hpp
+++ b/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)
{
- // 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;
+ 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;
}
diff --git a/thermosphere/src/traps/hvisor_traps_smc.cpp b/thermosphere/src/traps/hvisor_traps_smc.cpp
new file mode 100644
index 000000000..e04e4b815
--- /dev/null
+++ b/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 .
+ */
+
+#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(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(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);
+ }
+
+}
diff --git a/thermosphere/src/traps/smc.h b/thermosphere/src/traps/hvisor_traps_smc.hpp
similarity index 53%
rename from thermosphere/src/traps/smc.h
rename to thermosphere/src/traps/hvisor_traps_smc.hpp
index 0932a2bc6..94493e6ea 100644
--- a/thermosphere/src/traps/smc.h
+++ b/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);
+
+}
diff --git a/thermosphere/src/traps/smc.c b/thermosphere/src/traps/smc.c
deleted file mode 100644
index 11dbb9ac1..000000000
--- a/thermosphere/src/traps/smc.c
+++ /dev/null
@@ -1,75 +0,0 @@
-#include
-#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);
-}