target-arm: Implement AArch64 EL1 exception handling

Implement exception handling for AArch64 EL1. Exceptions from AArch64 or
AArch32 EL0 are supported.

Signed-off-by: Rob Herring <rob.herring@linaro.org>
[PMM: fixed minor style nits; updated to match changes in
 previous patches; added some of the simpler cases of
 illegal-exception-return support]
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>

6 files changed, 142 insertions, 0 deletions

diff --git a/target-arm/cpu-qom.h b/target-arm/cpu-qom.h
index 41caa6c780..afdee9d683 100644
--- a/target-arm/cpu-qom.h
+++ b/target-arm/cpu-qom.h
@@ -202,6 +202,8 @@ void aarch64_cpu_dump_state(CPUState *cs, FILE *f,
                             fprintf_function cpu_fprintf, int flags);
 int aarch64_cpu_gdb_read_register(CPUState *cpu, uint8_t *buf, int reg);
 int aarch64_cpu_gdb_write_register(CPUState *cpu, uint8_t *buf, int reg);
+
+void aarch64_cpu_do_interrupt(CPUState *cs);
 #endif
 
 #endif
diff --git a/target-arm/cpu64.c b/target-arm/cpu64.c
index fccecc2527..d4fb1de4fa 100644
--- a/target-arm/cpu64.c
+++ b/target-arm/cpu64.c
@@ -85,6 +85,7 @@ static void aarch64_cpu_class_init(ObjectClass *oc, void *data)
 {
     CPUClass *cc = CPU_CLASS(oc);
 
+    cc->do_interrupt = aarch64_cpu_do_interrupt;
     cc->dump_state = aarch64_cpu_dump_state;
     cc->set_pc = aarch64_cpu_set_pc;
     cc->gdb_read_register = aarch64_cpu_gdb_read_register;
diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 80ed029053..4f501b4040 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -23,6 +23,7 @@
 #include "qemu/host-utils.h"
 #include "sysemu/sysemu.h"
 #include "qemu/bitops.h"
+#include "internals.h"
 
 /* C2.4.7 Multiply and divide */
 /* special cases for 0 and LLONG_MIN are mandated by the standard */
@@ -288,3 +289,77 @@ float64 HELPER(rsqrtsf_f64)(float64 a, float64 b, void *fpstp)
     }
     return float64_muladd(a, b, float64_three, float_muladd_halve_result, fpst);
 }
+
+/* Handle a CPU exception.  */
+void aarch64_cpu_do_interrupt(CPUState *cs)
+{
+    ARMCPU *cpu = ARM_CPU(cs);
+    CPUARMState *env = &cpu->env;
+    target_ulong addr = env->cp15.c12_vbar;
+    int i;
+
+    if (arm_current_pl(env) == 0) {
+        if (env->aarch64) {
+            addr += 0x400;
+        } else {
+            addr += 0x600;
+        }
+    } else if (pstate_read(env) & PSTATE_SP) {
+        addr += 0x200;
+    }
+
+    arm_log_exception(env->exception_index);
+    qemu_log_mask(CPU_LOG_INT, "...from EL%d\n", arm_current_pl(env));
+    if (qemu_loglevel_mask(CPU_LOG_INT)
+        && !excp_is_internal(env->exception_index)) {
+        qemu_log_mask(CPU_LOG_INT, "...with ESR 0x%" PRIx32 "\n",
+                      env->exception.syndrome);
+    }
+
+    env->cp15.esr_el1 = env->exception.syndrome;
+    env->cp15.far_el1 = env->exception.vaddress;
+
+    switch (env->exception_index) {
+    case EXCP_PREFETCH_ABORT:
+    case EXCP_DATA_ABORT:
+        qemu_log_mask(CPU_LOG_INT, "...with FAR 0x%" PRIx64 "\n",
+                      env->cp15.far_el1);
+    case EXCP_BKPT:
+    case EXCP_UDEF:
+    case EXCP_SWI:
+        break;
+    case EXCP_IRQ:
+        addr += 0x80;
+        break;
+    case EXCP_FIQ:
+        addr += 0x100;
+        break;
+    default:
+        cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);
+    }
+
+    if (is_a64(env)) {
+        env->banked_spsr[0] = pstate_read(env);
+        env->sp_el[arm_current_pl(env)] = env->xregs[31];
+        env->xregs[31] = env->sp_el[1];
+        env->elr_el1 = env->pc;
+    } else {
+        env->banked_spsr[0] = cpsr_read(env);
+        if (!env->thumb) {
+            env->cp15.esr_el1 |= 1 << 25;
+        }
+        env->elr_el1 = env->regs[15];
+
+        for (i = 0; i < 15; i++) {
+            env->xregs[i] = env->regs[i];
+        }
+
+        env->condexec_bits = 0;
+    }
+
+    pstate_write(env, PSTATE_DAIF | PSTATE_MODE_EL1h);
+    env->aarch64 = 1;
+
+    env->pc = addr;
+    cs->interrupt_request |= CPU_INTERRUPT_EXITTB;
+}
diff --git a/target-arm/helper.h b/target-arm/helper.h
index 4c924e97d5..313d4cb893 100644
--- a/target-arm/helper.h
+++ b/target-arm/helper.h
@@ -65,6 +65,7 @@ DEF_HELPER_3(set_cp_reg64, void, env, ptr, i64)
 DEF_HELPER_2(get_cp_reg64, i64, env, ptr)
 
 DEF_HELPER_3(msr_i_pstate, void, env, i32, i32)
+DEF_HELPER_1(exception_return, void, env)
 
 DEF_HELPER_2(get_r13_banked, i32, env, i32)
 DEF_HELPER_3(set_r13_banked, void, env, i32, i32)
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index aeb2538b4c..5581f4fded 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -362,6 +362,66 @@ void HELPER(msr_i_pstate)(CPUARMState *env, uint32_t op, uint32_t imm)
     }
 }
 
+void HELPER(exception_return)(CPUARMState *env)
+{
+    uint32_t spsr = env->banked_spsr[0];
+    int new_el, i;
+
+    if (env->pstate & PSTATE_SP) {
+        env->sp_el[1] = env->xregs[31];
+    } else {
+        env->sp_el[0] = env->xregs[31];
+    }
+
+    env->exclusive_addr = -1;
+
+    if (spsr & PSTATE_nRW) {
+        env->aarch64 = 0;
+        new_el = 0;
+        env->uncached_cpsr = 0x10;
+        cpsr_write(env, spsr, ~0);
+        for (i = 0; i < 15; i++) {
+            env->regs[i] = env->xregs[i];
+        }
+
+        env->regs[15] = env->elr_el1 & ~0x1;
+    } else {
+        new_el = extract32(spsr, 2, 2);
+        if (new_el > 1) {
+            /* Return to unimplemented EL */
+            goto illegal_return;
+        }
+        if (extract32(spsr, 1, 1)) {
+            /* Return with reserved M[1] bit set */
+            goto illegal_return;
+        }
+        if (new_el == 0 && (spsr & PSTATE_SP)) {
+            /* Return to EL1 with M[0] bit set */
+            goto illegal_return;
+        }
+        env->aarch64 = 1;
+        pstate_write(env, spsr);
+        env->xregs[31] = env->sp_el[new_el];
+        env->pc = env->elr_el1;
+    }
+
+    return;
+
+illegal_return:
+    /* Illegal return events of various kinds have architecturally
+     * mandated behaviour:
+     * restore NZCV and DAIF from SPSR_ELx
+     * set PSTATE.IL
+     * restore PC from ELR_ELx
+     * no change to exception level, execution state or stack pointer
+     */
+    env->pstate |= PSTATE_IL;
+    env->pc = env->elr_el1;
+    spsr &= PSTATE_NZCV | PSTATE_DAIF;
+    spsr |= pstate_read(env) & ~(PSTATE_NZCV | PSTATE_DAIF);
+    pstate_write(env, spsr);
+}
+
 /* ??? Flag setting arithmetic is awkward because we need to do comparisons.
    The only way to do that in TCG is a conditional branch, which clobbers
    all our temporaries.  For now implement these as helper functions.  */
diff --git a/target-arm/translate-a64.c b/target-arm/translate-a64.c
index 4eaea6409c..50c5148bd2 100644
--- a/target-arm/translate-a64.c
+++ b/target-arm/translate-a64.c
@@ -1508,6 +1508,9 @@ static void disas_uncond_b_reg(DisasContext *s, uint32_t insn)
         tcg_gen_movi_i64(cpu_reg(s, 30), s->pc);
         break;
     case 4: /* ERET */
+        gen_helper_exception_return(cpu_env);
+        s->is_jmp = DISAS_JUMP;
+        return;
     case 5: /* DRPS */
         if (rn != 0x1f) {
             unallocated_encoding(s);