From 6d33ebceef98f3691841e412a9000b098cd2b4d4 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Wed, 31 Jul 2019 01:23:53 +0200
Subject: [PATCH] thermosphere: cpu_on hook & skeleton for other PSCI functions

---
 thermosphere/src/core_ctx.h |  2 +-
 thermosphere/src/smc.c      | 46 +++++++++++++++++++++++++++++++++++++
 thermosphere/src/start.s    |  8 ++++++-
 3 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h
index 0143eca57..41c18efe5 100644
--- a/thermosphere/src/core_ctx.h
+++ b/thermosphere/src/core_ctx.h
@@ -19,7 +19,7 @@
 
 typedef struct CoreCtx {
     u64 kernelArgument;
-    u64 kernelEntrypoint;
+    uintptr_t kernelEntrypoint;
     u32 coreId;             // @ 0x0C
 } CoreCtx;
 
diff --git a/thermosphere/src/smc.c b/thermosphere/src/smc.c
index 7a8eacbc2..682803218 100644
--- a/thermosphere/src/smc.c
+++ b/thermosphere/src/smc.c
@@ -1,11 +1,15 @@
 #include <string.h>
 #include "smc.h"
 #include "synchronization.h"
+#include "core_ctx.h"
 
 // Currently in exception_vectors.s:
 extern const u32 doSmcIndirectCallImpl[];
 extern const u32 doSmcIndirectCallImplSmcInstructionOffset, doSmcIndirectCallImplSize;
 
+// start.s
+void start2(u64 contextId);
+
 void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId)
 {
     u32 codebuf[doSmcIndirectCallImplSize]; // note: potential VLA
@@ -18,11 +22,53 @@ void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId)
     ((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] = (uintptr_t)start2;
+    }
+}
+
 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
+            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);
 }
diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s
index 26ea87828..815399666 100644
--- a/thermosphere/src/start.s
+++ b/thermosphere/src/start.s
@@ -29,6 +29,8 @@ _initialKernelEntrypoint:
 start:
     mov     x19, #1
     b       _startCommon
+.global start2
+.type   start2, %function
 start2:
     mov     x19, xzr
 _startCommon:
@@ -57,8 +59,12 @@ _startCommon:
     isb
 
     // Get core ID
+    // Ensure Aff0 is 4-1 at most (4 cores), and that Aff1, 2 and 3 are 0 (1 cluster only)
     mrs     x10, mpidr_el1
-    and     x10, x10, #0xFF
+    and     x10, x10, #0x00FFFFFF        // Aff0 to 2
+    and     x10, x10, #(0xFF << 32)      // Aff3
+    cmp     x10, #4
+    bhs     .
 
     // Set tmp stack (__stacks_top__ is aligned)
     adrp    x8, __stacks_top__