From 9ca6d1c16fbccd9e3b2adec32ebd790867c3490b Mon Sep 17 00:00:00 2001
From: Nick Chan <towinchenmi@gmail.com>
Date: Mon, 30 Sep 2024 15:29:34 +0800
Subject: [PATCH] exceptions: add el3_call()

Signed-off-by: Nick Chan <towinchenmi@gmail.com>
---
 proxyclient/m1n1/proxy.py | 5 +++++
 src/exception.c           | 4 ++++
 src/exception.h           | 1 +
 src/exception_asm.S       | 6 ++++++
 src/proxy.c               | 3 +++
 src/proxy.h               | 1 +
 src/smp.c                 | 6 ++++++
 src/smp.h                 | 5 ++++-
 src/startup.c             | 4 +++-
 9 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/proxyclient/m1n1/proxy.py b/proxyclient/m1n1/proxy.py
index 2ac99930..8428c8f5 100644
--- a/proxyclient/m1n1/proxy.py
+++ b/proxyclient/m1n1/proxy.py
@@ -494,6 +494,7 @@ class M1N1Proxy(Reloadable):
     P_PUT_SIMD_STATE = 0x00f
     P_REBOOT = 0x010
     P_SLEEP = 0x011
+    P_EL3_CALL = 0x012
 
     P_WRITE64 = 0x100
     P_WRITE32 = 0x101
@@ -760,6 +761,10 @@ class M1N1Proxy(Reloadable):
         self.request(self.P_REBOOT, no_reply=True)
     def sleep(self, deep=False):
         self.request(self.P_SLEEP, deep, no_reply=True)
+    def el3_call(self, addr, *args):
+        if len(args) > 4:
+            raise ValueError("Too many arguments")
+        return self.request(self.P_EL3_CALL, addr, *args)
 
     def write64(self, addr, data):
         '''write 8 byte value to given address'''
diff --git a/src/exception.c b/src/exception.c
index 944f1a24..5c01deed 100644
--- a/src/exception.c
+++ b/src/exception.c
@@ -281,6 +281,10 @@ void exc_sync(u64 *regs)
         // Monitor call
         u32 imm = mrs(ESR_EL3) & 0xffff;
         switch (imm) {
+            case 42:
+                regs[0] = ((uint64_t(*)(uint64_t, uint64_t, uint64_t, uint64_t))regs[0])(
+                    regs[1], regs[2], regs[3], regs[4]);
+                return;
             default:
                 printf("Unknown SMC: 0x%x\n", imm);
                 break;
diff --git a/src/exception.h b/src/exception.h
index 786f38f7..49c47b3b 100644
--- a/src/exception.h
+++ b/src/exception.h
@@ -49,6 +49,7 @@ void print_regs(u64 *regs, int el12);
 
 uint64_t el0_call(void *func, uint64_t a, uint64_t b, uint64_t c, uint64_t d);
 uint64_t el1_call(void *func, uint64_t a, uint64_t b, uint64_t c, uint64_t d);
+uint64_t el3_call(void *func, uint64_t a, uint64_t b, uint64_t c, uint64_t d);
 
 #endif
 
diff --git a/src/exception_asm.S b/src/exception_asm.S
index 5f22c029..5c4bdfae 100644
--- a/src/exception_asm.S
+++ b/src/exception_asm.S
@@ -242,3 +242,9 @@ _el1_vectors_start:
     hvc 0x1e
     .align 7
     hvc 0x1f
+
+.globl el3_call
+.type el3_call, @function
+el3_call:
+    smc 42
+    ret
\ No newline at end of file
diff --git a/src/proxy.c b/src/proxy.c
index 49b33825..c59225ca 100644
--- a/src/proxy.c
+++ b/src/proxy.c
@@ -116,6 +116,9 @@ int proxy_process(ProxyRequest *request, ProxyReply *reply)
         case P_SLEEP:
             cpu_sleep(request->args[0]);
             break;
+        case P_EL3_CALL:
+            reply->retval = el3_call((void *)request->args[0], request->args[1], request->args[2],
+                                     request->args[3], request->args[4]);
 
         case P_WRITE64:
             exc_guard = GUARD_SKIP;
diff --git a/src/proxy.h b/src/proxy.h
index c6c5006f..34c419ee 100644
--- a/src/proxy.h
+++ b/src/proxy.h
@@ -24,6 +24,7 @@ typedef enum {
     P_PUT_SIMD_STATE,
     P_REBOOT,
     P_SLEEP,
+    P_EL3_CALL,
 
     P_WRITE64 = 0x100, // Generic register functions
     P_WRITE32,
diff --git a/src/smp.c b/src/smp.c
index 0367e2e8..6e683b49 100644
--- a/src/smp.c
+++ b/src/smp.c
@@ -107,6 +107,12 @@ void smp_secondary_entry(void)
     }
 }
 
+void smp_secondary_prep_el3(void)
+{
+    msr(TPIDR_EL3, target_cpu);
+    return;
+}
+
 static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u64 cpu_start_base)
 {
     int i;
diff --git a/src/smp.h b/src/smp.h
index 223d2a31..02969786 100644
--- a/src/smp.h
+++ b/src/smp.h
@@ -14,6 +14,7 @@ extern u8 *secondary_stacks[MAX_CPUS];
 extern u8 *secondary_stacks_el3[MAX_EL3_CPUS];
 
 void smp_secondary_entry(void);
+void smp_secondary_prep_el3(void);
 
 void smp_start_secondaries(void);
 void smp_stop_secondaries(bool deep_sleep);
@@ -35,7 +36,9 @@ void smp_send_ipi(int cpu);
 
 static inline int smp_id(void)
 {
-    if (in_el2())
+    if (in_el3())
+        return mrs(TPIDR_EL3);
+    else if (in_el2())
         return mrs(TPIDR_EL2);
     else
         return mrs(TPIDR_EL1);
diff --git a/src/startup.c b/src/startup.c
index 70bf2589..e7bbd984 100644
--- a/src/startup.c
+++ b/src/startup.c
@@ -193,8 +193,10 @@ void _cpu_reset_c(void *stack)
 
     exception_initialize();
 
-    if (in_el3())
+    if (in_el3()) {
+        smp_secondary_prep_el3();
         return;
+    }
 
     if (!is_boot_cpu())
         smp_secondary_entry();