diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s index e60b8adc5..8da57b906 100644 --- a/thermosphere/src/exception_vectors.s +++ b/thermosphere/src/exception_vectors.s @@ -169,7 +169,39 @@ vector_entry fiq_sp0 vector_entry serror_sp0 bl unknown_exception - check_vector_size serror_sp0 + .endfunc + .cfi_endproc + /* To save space, insert in an unused vector segment. */ + +.global doSmcIndirectCallImpl +//.type doSmcIndirectCallImpl, %function +doSmcIndirectCallImpl: + 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] + + _doSmcIndirectCallImplSmcInstruction: + smc #0 + + 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 + +_doSmcIndirectCallImplEnd: +.global doSmcIndirectCallImplSmcInstructionOffset +doSmcIndirectCallImplSmcInstructionOffset: + .word _doSmcIndirectCallImplSmcInstruction - doSmcIndirectCallImpl +.global doSmcIndirectCallImplSize +doSmcIndirectCallImplSize: + .word _doSmcIndirectCallImplEnd - doSmcIndirectCallImpl /* Current EL, SPx */ vector_entry synch_spx diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index 6cd356018..1caf96c51 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -17,6 +17,7 @@ #include "hvc.h" #include "traps.h" #include "sysreg_traps.h" +#include "smc.h" #include "core_ctx.h" #include "log.h" @@ -107,12 +108,16 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg handleHypercall(frame, esr); break; #endif - case Exception_SystemRegisterTrap: - handleMsrMrsTrap(frame, esr); - break; case Exception_HypervisorCallA64: handleHypercall(frame, esr); break; + case Exception_MonitorCallA64: + handleSmcTrap(frame, esr); + break; + case Exception_SystemRegisterTrap: + handleMsrMrsTrap(frame, esr); + break; + default: serialLog("Lower EL sync exception, EC = 0x%02llx IL=%llu ISS=0x%06llx\n", (u64)esr.ec, esr.il, esr.iss); dumpStackFrame(frame, false); diff --git a/thermosphere/src/smc.c b/thermosphere/src/smc.c new file mode 100644 index 000000000..7a8eacbc2 --- /dev/null +++ b/thermosphere/src/smc.c @@ -0,0 +1,28 @@ +#include +#include "smc.h" +#include "synchronization.h" + +// Currently in exception_vectors.s: +extern const u32 doSmcIndirectCallImpl[]; +extern const u32 doSmcIndirectCallImplSmcInstructionOffset, doSmcIndirectCallImplSize; + +void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId) +{ + u32 codebuf[doSmcIndirectCallImplSize]; // note: potential VLA + memcpy(codebuf, doSmcIndirectCallImpl, doSmcIndirectCallImplSize); + codebuf[doSmcIndirectCallImplSmcInstructionOffset/4] |= smcId << 5; + + __dsb_sy(); + __isb(); + + ((void (*)(ExceptionStackFrame *))codebuf)(frame); +} + +void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) +{ + // TODO: Arm PSCI 0.2+ stuff + u32 smcId = esr.iss; + + doSmcIndirectCall(frame, smcId); + skipFaultingInstruction(frame, 4); +} diff --git a/thermosphere/src/smc.h b/thermosphere/src/smc.h new file mode 100644 index 000000000..0932a2bc6 --- /dev/null +++ b/thermosphere/src/smc.h @@ -0,0 +1,23 @@ +/* + * Copyright (c) 2019 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 . + */ + +#pragma once + +#include "traps.h" + +void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId); + +void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);