exception: Preserve PSTATE bits on EL0 & EL1 calls

Signed-off-by: Hector Martin <marcan@marcan.st>

Author: marcan

src/exception.c

@@ -243,18 +243,33 @@ void exc_sync(u64 *regs)
     u64 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1) : (el3 ? mrs(ESR_EL3) : mrs(ESR_EL1));
     u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1) : (el3 ? mrs(ELR_EL3) : mrs(ELR_EL1));
 
-    if ((spsr & 0xf) == 0 && ((esr >> 26) & 0x3f) == 0x3c) {
+    u32 iss = esr & 0xffffff;
+
+    if ((spsr & 0xf) == 0 && ((esr >> 26) & 0x3f) == 0x3c && iss == 0) {
+        // brk 0
         // On clean EL0 return, let the normal exception return
         // path take us back to the return thunk.
-        if (has_el2())
-            msr(SPSR_EL1, 0x09 | (0xf << 6)); // EL2h
-        else
-            msr(SPSR_EL1, 0x05 | (0xf << 6)); // EL1h
+
+        spsr &= ~0x1f;
+        spsr |= 0xf << 6;
+        if (has_el2()) {
+            spsr |= 0x09; // EL2h
+        } else {
+            spsr |= 0x05; // EL1h
+        }
+
+        msr(SPSR_EL1, spsr);
 
         msr(ELR_EL1, el0_ret);
         return;
     }
 
+    if (((esr >> 26) & 0x3f) == 0x3c && iss == 1) {
+        // brk 1: Capture PSTATE
+        regs[0] = spsr;
+        return;
+    }
+
     if (in_el2() && !in_gl12() && (spsr & 0xf) == 5 && ((esr >> 26) & 0x3f) == 0x16) {
         // Hypercall
         u32 imm = mrs(ESR_EL2) & 0xffff;

src/exception_asm.S

@@ -112,9 +112,14 @@ el0_call:
     isb
 
     1:
-    mrs x29, daif
+    // Capture PSTATE
+    mov x6, x0
+    brk 1
+    mov x29, x0
+    bic x5, x0, #0xf
     msr daifset, #0xf
-    msr spsr_el1, x29
+    msr spsr_el1, x5
+    mov x0, x6
 
     ldr x5, =_el0_thunk
     msr elr_el1, x5
@@ -170,11 +175,16 @@ el1_call:
     isb
 
     1:
-    mrs x29, daif
-    msr daifset, #0xf
+    // Capture PSTATE
+    mov x6, x0
+    brk 1
+    mov x29, x0
+    bic x5, x0, #0xf
     mov x6, #5
-    orr x5, x29, x6 // EL1h
-    msr spsr_el2, x5
+    orr x5, x5, x6 // EL1h
+    msr daifset, #0xf
+    msr spsr_el1, x5
+    mov x0, x6
 
     ldr x5, =_el1_thunk
     msr elr_el2, x5