optimize: CPU Pipeline optimization part 1

In preperation for later optimization in the CPU pipeline
implementation, this commit refactors the arm/thumb exec functions to return a
CpuAction (Whether to advance the program counter or to flush the
pipeline)

Currently, a lot of host cycles are wasted in the arm7tdmi pipeline
Refill1 & Refill2 states. Optimizing these steps out would make the CPU
a bit faster.


Former-commit-id: 9be7966eaad22cceeb443fcc5823bbd945284027

src/core/arm7tdmi/arm/exec.rs

@@ -2,6 +2,7 @@ use crate::bit::BitIndex;
 
 use super::super::alu::*;
 use crate::core::arm7tdmi::psr::RegPSR;
+use crate::core::arm7tdmi::CpuAction;
 use crate::core::arm7tdmi::{Core, Addr, CpuMode, CpuState, REG_LR, REG_PC};
 use crate::core::sysbus::SysBus;
 use crate::core::Bus;
@@ -9,10 +10,10 @@ use crate::core::Bus;
 use super::*;
 
 impl Core {
-    pub fn exec_arm(&mut self, bus: &mut SysBus, insn: ArmInstruction) {
+    pub fn exec_arm(&mut self, bus: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         if !self.check_arm_cond(insn.cond) {
             self.S_cycle32(bus, self.pc);
-            return;
+            return CpuAction::AdvancePC;
         }
         match insn.fmt {
             ArmFormat::BX => self.exec_bx(bus, insn),
@@ -20,6 +21,7 @@ impl Core {
             ArmFormat::DP => self.exec_data_processing(bus, insn),
             ArmFormat::SWI => {
                 self.software_interrupt(bus, self.pc - 4, insn.swi_comment());
+                CpuAction::FlushPipeline
             }
             ArmFormat::LDR_STR => self.exec_ldr_str(bus, insn),
             ArmFormat::LDR_STR_HS_IMM => self.exec_ldr_str_hs(bus, insn),
@@ -35,18 +37,18 @@ impl Core {
     }
 
     /// Cycles 2S+1N
-    fn exec_b_bl(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_b_bl(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         self.S_cycle32(sb, self.pc);
         if insn.link_flag() {
             self.set_reg(REG_LR, (insn.pc + (self.word_size() as u32)) & !0b1);
         }
 
         self.pc = (self.pc as i32).wrapping_add(insn.branch_offset()) as u32 & !1;
-        self.flush_pipeline32(sb);
 
+        CpuAction::FlushPipeline
     }
 
-    pub fn branch_exchange(&mut self, sb: &mut SysBus, mut addr: Addr) {
+    pub fn branch_exchange(&mut self, sb: &mut SysBus, mut addr: Addr) -> CpuAction {
         match self.cpsr.state() {
             CpuState::ARM => self.S_cycle32(sb, self.pc),
             CpuState::THUMB => self.S_cycle16(sb, self.pc),
@@ -54,18 +56,18 @@ impl Core {
         if addr.bit(0) {
             addr = addr & !0x1;
             self.cpsr.set_state(CpuState::THUMB);
-            self.pc = addr;
-            self.flush_pipeline16(sb);
         } else {
             addr = addr & !0x3;
             self.cpsr.set_state(CpuState::ARM);
-            self.pc = addr;
-            self.flush_pipeline32(sb);
         }
+
+        self.pc = addr;
+
+        CpuAction::FlushPipeline
     }
 
     /// Cycles 2S+1N
-    fn exec_bx(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_bx(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         self.branch_exchange(sb, self.get_reg(insn.rn()))
     }
 
@@ -74,7 +76,7 @@ impl Core {
         sb: &mut SysBus,
         rd: usize,
         is_spsr: bool,
-    ) {
+    ) -> CpuAction {
         let result = if is_spsr {
             self.spsr.get()
         } else {
@@ -82,9 +84,11 @@ impl Core {
         };
         self.set_reg(rd, result);
         self.S_cycle32(sb, self.pc);
+
+        CpuAction::AdvancePC
     }
 
-    fn exec_msr_reg(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_msr_reg(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         self.write_status_register(sb, insn.spsr_flag(), self.get_reg(insn.rm()))
     }
 
@@ -93,7 +97,7 @@ impl Core {
         sb: &mut SysBus,
         is_spsr: bool,
         value: u32,
-    ) {
+    ) -> CpuAction {
         let new_status_reg = RegPSR::new(value);
         match self.cpsr.mode() {
             CpuMode::User => {
@@ -120,9 +124,11 @@ impl Core {
             }
         }
         self.S_cycle32(sb, self.pc);
+
+        CpuAction::AdvancePC
     }
 
-    fn exec_msr_flags(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_msr_flags(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         self.S_cycle32(sb, self.pc);
         let op = insn.operand2();
         let op = self.decode_operand2(op);
@@ -132,6 +138,7 @@ impl Core {
         } else {
             self.cpsr.set_flag_bits(op);
         }
+        CpuAction::AdvancePC
     }
 
     fn decode_operand2(&mut self, op2: BarrelShifterValue) -> u32 {
@@ -158,7 +165,7 @@ impl Core {
     ///
     /// Cycles: 1S+x+y (from GBATEK)
     ///         Add x=1I cycles if Op2 shifted-by-register. Add y=1S+1N cycles if Rd=R15.
-    fn exec_data_processing(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_data_processing(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         use AluOpCode::*;
 
         self.S_cycle32(sb, self.pc);
@@ -246,13 +253,15 @@ impl Core {
             })
         };
 
-        if let Some(result) = alu_res {
+        let mut result = CpuAction::AdvancePC;
+        if let Some(alu_res) = alu_res {
+            self.set_reg(reg_rd, alu_res as u32);
             if reg_rd == REG_PC {
-                self.flush_pipeline32(sb);
+                result = CpuAction::FlushPipeline;
             }
-            self.set_reg(reg_rd, result as u32);
         }
 
+        result
     }
 
     /// Memory Load/Store
@@ -262,7 +271,9 @@ impl Core {
     /// STR{cond}{B}{T} Rd,<Address>    | 2N            | ----  |  [Rn+/-<offset>]=Rd
     /// ------------------------------------------------------------------------------
     /// For LDR, add y=1S+1N if Rd=R15.
-    fn exec_ldr_str(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_ldr_str(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
+        let mut result = CpuAction::AdvancePC;
+
         let load = insn.load_flag();
         let pre_index = insn.pre_index_flag();
         let writeback = insn.write_back_flag();
@@ -303,7 +314,7 @@ impl Core {
             self.add_cycle();
 
             if dest_reg == REG_PC {
-                self.flush_pipeline32(sb);
+                result = CpuAction::FlushPipeline;
             }
         } else {
             let value = if dest_reg == REG_PC {
@@ -333,9 +344,12 @@ impl Core {
             self.change_mode(self.cpsr.mode(), old_mode);
         }
 
+        result
     }
 
-    fn exec_ldr_str_hs(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_ldr_str_hs(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
+        let mut result = CpuAction::AdvancePC;
+
         let load = insn.load_flag();
         let pre_index = insn.pre_index_flag();
         let writeback = insn.write_back_flag();
@@ -384,7 +398,7 @@ impl Core {
             self.add_cycle();
 
             if dest_reg == REG_PC {
-                self.flush_pipeline32(sb);
+                result = CpuAction::FlushPipeline;
             }
         } else {
             let value = if dest_reg == REG_PC {
@@ -411,9 +425,12 @@ impl Core {
             }
         }
 
+        result
     }
 
-    fn exec_ldm_stm(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_ldm_stm(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
+        let mut result = CpuAction::AdvancePC;
+
         let mut full = insn.pre_index_flag();
         let ascending = insn.add_offset_flag();
         let s_flag = insn.raw.bit(22);
@@ -487,7 +504,7 @@ impl Core {
                             if psr_transfer {
                                 self.transfer_spsr_mode();
                             }
-                            self.flush_pipeline32(sb);
+                            result = CpuAction::FlushPipeline;
                         }
 
                         if !full {
@@ -541,7 +558,7 @@ impl Core {
             if is_load {
                 let val = self.ldr_word(addr, sb);
                 self.set_reg(REG_PC, val & !3);
-                self.flush_pipeline32(sb);
+                result = CpuAction::FlushPipeline;
             } else {
                 self.write_32(addr, self.pc + 4, sb);
             }
@@ -556,9 +573,10 @@ impl Core {
             self.set_reg(base_reg, addr as u32);
         }
 
+        result
     }
 
-    fn exec_mul_mla(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_mul_mla(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         let (rd, rn, rs, rm) = (insn.rd(), insn.rn(), insn.rs(), insn.rm());
 
         // check validity
@@ -589,9 +607,11 @@ impl Core {
         }
 
         self.S_cycle32(sb, self.pc);
+
+        CpuAction::AdvancePC
     }
 
-    fn exec_mull_mlal(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_mull_mlal(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         let (rd_hi, rd_lo, rn, rs, rm) =
             (insn.rd_hi(), insn.rd_lo(), insn.rn(), insn.rs(), insn.rm());
 
@@ -632,11 +652,12 @@ impl Core {
         }
 
         self.S_cycle32(sb, self.pc);
+
+        CpuAction::AdvancePC
     }
 
-    fn exec_arm_swp(&mut self, sb: &mut SysBus, insn: ArmInstruction) {
+    fn exec_arm_swp(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuAction {
         let base_addr = self.get_reg(insn.rn());
-        self.add_cycle();
         if insn.transfer_size() == 1 {
             let t = sb.read_8(base_addr);
             self.N_cycle8(sb, base_addr);
@@ -650,6 +671,9 @@ impl Core {
             self.S_cycle32(sb, base_addr);
             self.set_reg(insn.rd(), t as u32);
         }
+        self.add_cycle();
         self.N_cycle32(sb, self.pc);
+
+        CpuAction::AdvancePC
     }
 }

src/core/arm7tdmi/cpu.rs

@@ -6,6 +6,7 @@ use ansi_term::{Colour, Style};
 use super::reg_string;
 use serde::{Deserialize, Serialize};
 
+use super::CpuAction;
 pub use super::exception::Exception;
 use super::{
     arm::*, psr::RegPSR, thumb::ThumbInstruction, Addr, CpuMode, CpuResult, CpuState,
@@ -312,6 +313,23 @@ impl Core {
         self.pipeline_state != PipelineState::Execute
     }
 
+    fn handle_exec_result(&mut self, sb: &mut SysBus, exec_result: CpuAction) {
+        match self.cpsr.state() {
+            CpuState::ARM => {
+                match exec_result {
+                    CpuAction::AdvancePC => self.advance_arm(),
+                    CpuAction::FlushPipeline => self.flush_pipeline32(sb),
+                }
+            }
+            CpuState::THUMB => {
+                match exec_result {
+                    CpuAction::AdvancePC => self.advance_thumb(),
+                    CpuAction::FlushPipeline => self.flush_pipeline16(sb),
+                }
+            }
+        }
+    }
+
     fn step_arm_exec(&mut self, insn: u32, sb: &mut SysBus) {
         let pc = self.pc;
         match self.pipeline_state {
@@ -331,11 +349,9 @@ impl Core {
                 {
                     self.gpr_previous = self.get_registers();
                 }
-                self.exec_arm(sb, decoded_arm);
-                if !self.did_pipeline_flush() {
-                    self.pc = pc.wrapping_add(4);
-                }
                 self.last_executed = Some(DecodedInstruction::Arm(decoded_arm));
+                let result = self.exec_arm(sb, decoded_arm);
+                self.handle_exec_result(sb, result);
             }
         }
     }
@@ -359,27 +375,37 @@ impl Core {
                 {
                     self.gpr_previous = self.get_registers();
                 }
-                self.exec_thumb(sb, decoded_thumb);
-                if !self.did_pipeline_flush() {
-                    self.pc = pc.wrapping_add(2);
-                }
                 self.last_executed = Some(DecodedInstruction::Thumb(decoded_thumb));
+                let result = self.exec_thumb(sb, decoded_thumb);
+                self.handle_exec_result(sb, result);
             }
         }
     }
 
+    #[inline]
     pub(super) fn flush_pipeline16(&mut self, sb: &mut SysBus) {
         self.pipeline_state = PipelineState::Refill1;
         self.N_cycle16(sb, self.pc);
         self.S_cycle16(sb, self.pc + 2);
     }
 
+    #[inline]
     pub(super) fn flush_pipeline32(&mut self, sb: &mut SysBus) {
         self.pipeline_state = PipelineState::Refill1;
         self.N_cycle32(sb, self.pc);
         self.S_cycle32(sb, self.pc + 4);
     }
 
+    #[inline]
+    pub(super) fn advance_thumb(&mut self) {
+        self.pc = self.pc.wrapping_add(2)
+    }
+
+    #[inline]
+    pub(super) fn advance_arm(&mut self) {
+        self.pc = self.pc.wrapping_add(4)
+    }
+
     // fn trace_opcode(&self, insn: u32) {
     //     if self.trace_opcodes && self.pipeline_state == PipelineState::Execute {
     //         println!("[{:08X}] PC=0x{:08x} | ", insn, self.pc);

src/core/arm7tdmi/exception.rs

@@ -56,7 +56,6 @@ impl Core {
 
         // Set PC to vector address
         self.pc = e as u32;
-        self.flush_pipeline32(sb);
     }
 
     pub fn irq(&mut self, sb: &mut SysBus) {
@@ -65,7 +64,8 @@ impl Core {
         }
         if !self.cpsr.irq_disabled() {
             let lr = self.get_next_pc() + 4;
-            self.exception(sb, Exception::Irq, lr)
+            self.exception(sb, Exception::Irq, lr);
+            self.flush_pipeline32(sb);
         }
     }
 

src/core/arm7tdmi/mod.rs

@@ -23,6 +23,11 @@ pub const REG_SP: usize = 13;
 
 pub(self) use crate::core::Addr;
 
+pub enum CpuAction {
+    AdvancePC,
+    FlushPipeline
+}
+
 #[derive(Serialize, Deserialize, Debug, PartialEq, Copy, Clone)]
 pub enum DecodedInstruction {
     Arm(ArmInstruction),

src/core/arm7tdmi/thumb/exec.rs

@@ -22,7 +22,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    )  -> CpuAction {
         let rd = (insn.raw & 0b111) as usize;
         let rs = insn.raw.bit_range(3..6) as usize;
 
@@ -33,10 +33,12 @@ impl Core {
         self.alu_update_flags(op2, false, self.bs_carry_out, self.cpsr.V());
 
         self.S_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 2
-    fn exec_thumb_add_sub(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_add_sub(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let rd = (insn.raw & 0b111) as usize;
         let op1 = self.get_reg(insn.rs());
         let op2 = if insn.is_immediate_operand() {
@@ -56,6 +58,8 @@ impl Core {
         self.set_reg(rd, result as u32);
 
         self.S_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 3
@@ -63,7 +67,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         use OpFormat3::*;
         let op = insn.format3_op();
         let rd = insn.raw.bit_range(8..11) as usize;
@@ -82,10 +86,12 @@ impl Core {
             self.gpr[rd] = result as u32;
         }
         self.S_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 4
-    fn exec_thumb_alu_ops(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_alu_ops(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let rd = (insn.raw & 0b111) as usize;
         let rs = insn.rs();
         let dst = self.get_reg(rd);
@@ -137,6 +143,8 @@ impl Core {
             self.set_reg(rd, result as u32);
         }
         self.S_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 5
@@ -144,7 +152,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         let op = insn.format5_op();
         let rd = (insn.raw & 0b111) as usize;
         let dst_reg = if insn.flag(ThumbInstruction::FLAG_H1) {
@@ -160,15 +168,15 @@ impl Core {
         let op1 = self.get_reg(dst_reg);
         let op2 = self.get_reg(src_reg);
 
+        let mut result = CpuAction::AdvancePC;
         match op {
             OpFormat5::BX => {
-                self.branch_exchange(sb, self.get_reg(src_reg));
+                return self.branch_exchange(sb, self.get_reg(src_reg));
-                return;
             },
             OpFormat5::ADD => {
                 self.set_reg(dst_reg, op1.wrapping_add(op2));
                 if dst_reg == REG_PC {
-                    self.flush_pipeline16(sb);
+                    result = CpuAction::FlushPipeline
                 }
             }
             OpFormat5::CMP => {
@@ -180,15 +188,17 @@ impl Core {
             OpFormat5::MOV => {
                 self.set_reg(dst_reg, op2 as u32);
                 if dst_reg == REG_PC {
-                    self.flush_pipeline16(sb);
+                    result = CpuAction::FlushPipeline;
                 }
             }
         }
         self.S_cycle16(sb, self.pc + 2);
+
+        result
     }
 
     /// Format 6
-    fn exec_thumb_ldr_pc(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_ldr_pc(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let rd = insn.raw.bit_range(8..11) as usize;
 
         let ofs = insn.word8() as Addr;
@@ -203,6 +213,7 @@ impl Core {
         // +1I
         self.add_cycle();
 
+        CpuAction::AdvancePC
     }
 
     fn do_exec_thumb_ldr_str(
@@ -211,7 +222,7 @@ impl Core {
         insn: ThumbInstruction,
         addr: Addr,
         is_transferring_bytes: bool,
-    )  {
+    ) -> CpuAction {
         let rd = (insn.raw & 0b111) as usize;
         if insn.is_load() {
             let data = if is_transferring_bytes {
@@ -238,6 +249,8 @@ impl Core {
         }
 
         self.N_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 7
@@ -245,14 +258,14 @@ impl Core {
         &mut self,
         bus: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         let rb = insn.raw.bit_range(3..6) as usize;
         let addr = self.gpr[rb].wrapping_add(self.gpr[insn.ro()]);
         self.do_exec_thumb_ldr_str(bus, insn, addr, insn.raw.bit(10))
     }
 
     /// Format 8
-    fn exec_thumb_ldr_str_shb(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_ldr_str_shb(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let rb = insn.raw.bit_range(3..6) as usize;
         let rd = (insn.raw & 0b111) as usize;
 
@@ -293,6 +306,8 @@ impl Core {
         }
 
         self.N_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 9
@@ -300,7 +315,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         let rb = insn.raw.bit_range(3..6) as usize;
 
         let offset = if insn.raw.bit(12) {
@@ -317,7 +332,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         let rb = insn.raw.bit_range(3..6) as usize;
         let rd = (insn.raw & 0b111) as usize;
         let base = self.gpr[rb] as i32;
@@ -332,10 +347,12 @@ impl Core {
             self.N_cycle16(sb, addr);
         }
         self.N_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 11
-    fn exec_thumb_ldr_str_sp(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_ldr_str_sp(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let addr = self.gpr[REG_SP] + (insn.word8() as Addr);
         let rd = insn.raw.bit_range(8..11) as usize;
         if insn.is_load() {
@@ -348,6 +365,8 @@ impl Core {
             self.N_cycle16(sb, addr);
         }
         self.N_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 12
@@ -355,7 +374,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         let rd = insn.raw.bit_range(8..11) as usize;
         let result = if insn.flag(ThumbInstruction::FLAG_SP) {
             self.gpr[REG_SP] + (insn.word8() as Addr)
@@ -364,19 +383,25 @@ impl Core {
         };
         self.gpr[rd] = result;
         self.S_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 13
-    fn exec_thumb_add_sp(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_add_sp(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let op1 = self.gpr[REG_SP] as i32;
         let op2 = insn.sword7();
 
         self.gpr[REG_SP] = op1.wrapping_add(op2) as u32;
         self.S_cycle16(sb, self.pc + 2);
+
+        CpuAction::AdvancePC
     }
 
     /// Format 14
-    fn exec_thumb_push_pop(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_push_pop(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
+        let mut result = CpuAction::AdvancePC;
+
         // (From GBATEK) Execution Time: nS+1N+1I (POP), (n+1)S+2N+1I (POP PC), or (n-1)S+2N (PUSH).
         let is_pop = insn.is_load();
         let pc_lr_flag = insn.flag(ThumbInstruction::FLAG_R);
@@ -398,7 +423,7 @@ impl Core {
             if pc_lr_flag {
                 pop(self, sb, REG_PC);
                 self.pc = self.pc & !1;
-                self.flush_pipeline16(sb);
+                result = CpuAction::FlushPipeline;
             }
             self.S_cycle16(sb, self.pc + 2);
         } else {
@@ -417,10 +442,13 @@ impl Core {
             }
         }
 
+        result
     }
 
     /// Format 15
-    fn exec_thumb_ldm_stm(&mut self, sb: &mut SysBus, insn: ThumbInstruction)  {
+    fn exec_thumb_ldm_stm(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
+        let mut result = CpuAction::AdvancePC;
+
         // (From GBATEK) Execution Time: nS+1N+1I (POP), (n+1)S+2N+1I (POP PC), or (n-1)S+2N (PUSH).
 
         let rb = insn.raw.bit_range(8..11) as usize;
@@ -482,7 +510,7 @@ impl Core {
             if is_load {
                 let val = sb.read_32(addr);
                 self.set_reg(REG_PC, val & !1);
-                self.flush_pipeline16(sb);
+                result = CpuAction::FlushPipeline;
             } else {
                 sb.write_32(addr, self.pc + 2);
             }
@@ -490,6 +518,7 @@ impl Core {
             self.gpr[base_reg] = addr + align_preserve;
         }
 
+        result
     }
 
     /// Format 16
@@ -497,14 +526,15 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         if !self.check_arm_cond(insn.cond()) {
             self.S_cycle16(sb, self.pc + 2);
+            CpuAction::AdvancePC
         } else {
             let offset = insn.bcond_offset();
             self.S_cycle16(sb, self.pc);
             self.pc = (self.pc as i32).wrapping_add(offset) as u32;
-            self.flush_pipeline16(sb);
+            CpuAction::FlushPipeline
         }
     }
 
@@ -513,17 +543,20 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         _insn: ThumbInstruction,
-    )  {
+    ) -> CpuAction {
         self.N_cycle16(sb, self.pc);
         self.exception(sb, Exception::SoftwareInterrupt, self.pc - 2);
+
+        CpuAction::FlushPipeline
     }
 
     /// Format 18
-    fn exec_thumb_branch(&mut self, sb: &mut SysBus, insn: ThumbInstruction) {
+    fn exec_thumb_branch(&mut self, sb: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         let offset = ((insn.offset11() << 21) >> 20) as i32;
         self.pc = (self.pc as i32).wrapping_add(offset) as u32;
         self.S_cycle16(sb, self.pc);
-        self.flush_pipeline16(sb);
+
+        CpuAction::FlushPipeline
     }
 
     /// Format 19
@@ -531,7 +564,7 @@ impl Core {
         &mut self,
         sb: &mut SysBus,
         insn: ThumbInstruction,
-    ) {
+    ) -> CpuAction {
         let mut off = insn.offset11();
         if insn.flag(ThumbInstruction::FLAG_LOW_OFFSET) {
             self.S_cycle16(sb, self.pc);
@@ -540,15 +573,17 @@ impl Core {
             self.pc = ((self.gpr[REG_LR] & !1) as i32).wrapping_add(off) as u32;
             self.gpr[REG_LR] = next_pc;
 
-            self.flush_pipeline16(sb);
+            CpuAction::FlushPipeline
         } else {
             off = (off << 21) >> 9;
             self.gpr[REG_LR] = (self.pc as i32).wrapping_add(off) as u32;
             self.S_cycle16(sb, self.pc);
+
+            CpuAction::AdvancePC
         }
     }
 
-    pub fn exec_thumb(&mut self, bus: &mut SysBus, insn: ThumbInstruction) {
+    pub fn exec_thumb(&mut self, bus: &mut SysBus, insn: ThumbInstruction) -> CpuAction {
         match insn.fmt {
             ThumbFormat::MoveShiftedReg => self.exec_thumb_move_shifted_reg(bus, insn),
             ThumbFormat::AddSub => self.exec_thumb_add_sub(bus, insn),