From 4a5d05f32bbd23271fd6382e419ce4f11ae144f2 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Wed, 31 Jul 2019 00:08:09 +0200
Subject: [PATCH] thermosphere: add smc trap handler

---
 thermosphere/src/exception_vectors.s | 34 +++++++++++++++++++++++++++-
 thermosphere/src/exceptions.c        | 11 ++++++---
 thermosphere/src/smc.c               | 28 +++++++++++++++++++++++
 thermosphere/src/smc.h               | 23 +++++++++++++++++++
 4 files changed, 92 insertions(+), 4 deletions(-)
 create mode 100644 thermosphere/src/smc.c
 create mode 100644 thermosphere/src/smc.h

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 <string.h>
+#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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "traps.h"
+
+void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId);
+
+void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);