diff --git a/src/exception.c b/src/exception.c index 8588aeb9..944f1a24 100644 --- a/src/exception.c +++ b/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; } diff --git a/src/exception_asm.S b/src/exception_asm.S index 8c8d01f9..5f22c029 100644 --- a/src/exception_asm.S +++ b/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 diff --git a/src/utils.h b/src/utils.h index 6ce9e042..28403b6c 100644 --- a/src/utils.h +++ b/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;