OmniNX/Atmosphere
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

thermosphere: rewrite smc traps

Browse Source
This commit is contained in:
TuxSH
2020-02-26 01:18:15 +00:00
parent 0437867449
commit e8435784a7
6 changed files with 210 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
thermosphere/src/traps/hvisor_traps_smc.cpp Normal file
View File

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

18
thermosphere/src/traps/smc.h → thermosphere/src/traps/hvisor_traps_smc.hpp
View File

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

75
thermosphere/src/traps/smc.c
View File

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