3 files changed, 36 insertions, 0 deletions
diff --git a/target-arm/cpu-qom.h b/target-arm/cpu-qom.h
index 07f3c9e866..96a3da9a8f 100644
--- a/target-arm/cpu-qom.h
+++ b/target-arm/cpu-qom.h
@@ -192,6 +192,7 @@ void init_cpreg_list(ARMCPU *cpu);
 
 void arm_cpu_do_interrupt(CPUState *cpu);
 void arm_v7m_cpu_do_interrupt(CPUState *cpu);
+bool arm_cpu_exec_interrupt(CPUState *cpu, int int_req);
 
 void arm_cpu_dump_state(CPUState *cs, FILE *f, fprintf_function cpu_fprintf,
                         int flags);
diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index 7ea12bda1c..407f977742 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -188,6 +188,39 @@ static void arm_cpu_reset(CPUState *s)
     hw_watchpoint_update_all(cpu);
 }
 
+bool arm_cpu_exec_interrupt(CPUState *cs, int interrupt_request)
+{
+    CPUClass *cc = CPU_GET_CLASS(cs);
+    ARMCPU *cpu = ARM_CPU(cs);
+    CPUARMState *env = &cpu->env;
+    bool ret = false;
+
+    if (interrupt_request & CPU_INTERRUPT_FIQ
+        && !(env->daif & PSTATE_F)) {
+        cs->exception_index = EXCP_FIQ;
+        cc->do_interrupt(cs);
+        ret = true;
+    }
+    /* ARMv7-M interrupt return works by loading a magic value
+       into the PC.  On real hardware the load causes the
+       return to occur.  The qemu implementation performs the
+       jump normally, then does the exception return when the
+       CPU tries to execute code at the magic address.
+       This will cause the magic PC value to be pushed to
+       the stack if an interrupt occurred at the wrong time.
+       We avoid this by disabling interrupts when
+       pc contains a magic address.  */
+    if (interrupt_request & CPU_INTERRUPT_HARD
+        && !(env->daif & PSTATE_I)
+        && (!IS_M(env) || env->regs[15] < 0xfffffff0)) {
+        cs->exception_index = EXCP_IRQ;
+        cc->do_interrupt(cs);
+        ret = true;
+    }
+
+    return ret;
+}
+
 #ifndef CONFIG_USER_ONLY
 static void arm_cpu_set_irq(void *opaque, int irq, int level)
 {
@@ -1053,6 +1086,7 @@ static void arm_cpu_class_init(ObjectClass *oc, void *data)
     cc->class_by_name = arm_cpu_class_by_name;
     cc->has_work = arm_cpu_has_work;
     cc->do_interrupt = arm_cpu_do_interrupt;
+    cc->cpu_exec_interrupt = arm_cpu_exec_interrupt;
     cc->dump_state = arm_cpu_dump_state;
     cc->set_pc = arm_cpu_set_pc;
     cc->gdb_read_register = arm_cpu_gdb_read_register;
diff --git a/target-arm/cpu64.c b/target-arm/cpu64.c
index aa42803959..c30f47eca7 100644
--- a/target-arm/cpu64.c
+++ b/target-arm/cpu64.c
@@ -197,6 +197,7 @@ static void aarch64_cpu_class_init(ObjectClass *oc, void *data)
     CPUClass *cc = CPU_CLASS(oc);
 
     cc->do_interrupt = aarch64_cpu_do_interrupt;
+    cc->cpu_exec_interrupt = arm_cpu_exec_interrupt;
     cc->set_pc = aarch64_cpu_set_pc;
     cc->gdb_read_register = aarch64_cpu_gdb_read_register;
     cc->gdb_write_register = aarch64_cpu_gdb_write_register;