exceptions: fix el0_call() and el1_call() when running in EL1

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

src/exception.c

@@ -246,7 +246,11 @@ void exc_sync(u64 *regs)
     if ((spsr & 0xf) == 0 && ((esr >> 26) & 0x3f) == 0x3c) {
         // On clean EL0 return, let the normal exception return
         // path take us back to the return thunk.
-        msr(SPSR_EL1, 0x09); // EL2h
+        if (has_el2())
+            msr(SPSR_EL1, 0x09); // EL2h
+        else
+            msr(SPSR_EL1, 0x05); // EL1h
+
         msr(ELR_EL1, el0_ret);
         return;
     }

src/exception_asm.S

@@ -101,12 +101,17 @@ _exc_return:
 el0_call:
     str x30, [sp, #-16]!
 
+    mrs x5, CurrentEL
+    cmp x5, #4
+    beq 1f
+
     // Disable EL1
     mrs x5, hcr_el2
     orr x5, x5, #(1 << 27)
     msr hcr_el2, x5
     isb
 
+    1:
     mrs x5, daif
     msr daifclr, 3
     msr spsr_el1, x5
@@ -149,6 +154,10 @@ el0_ret:
 el1_call:
     str x30, [sp, #-16]!
 
+    mrs x5, CurrentEL
+    cmp x5, #4
+    beq _el1_thunk
+
     // Enable EL1, but only if not already done.
     // this check is here because writes to hcr_el2 are only possible from GL2
     // if that mode has been enabled
@@ -183,6 +192,10 @@ _el1_thunk:
 
     blr x5
 
+    mrs x5, CurrentEL
+    cmp x5, #4
+    beq el1_ret
+
     hvc 0
     .long 0
 

src/utils.h

@@ -353,6 +353,11 @@ static inline int has_el3(void)
     return !!(mrs(ID_AA64PFR0_EL1) & 0xf000);
 }
 
+static inline int has_el2(void)
+{
+    return !!(mrs(ID_AA64PFR0_EL1) & 0xf00);
+}
+
 static inline bool is_16k(void)
 {
     return ((mrs(ID_AA64MMFR0_EL1) >> 20) & 0xf) == 0x1;