exceptions: add el3_call()

Signed-off-by: Nick Chan <towinchenmi@gmail.com>

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'''

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;

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
 

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

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;

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,

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;

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

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();