mGBA test suite now boots!

Fix tons of bug and reimplemented some of the core code.
Add a neat feature for debug builds:
When the cpu "swi 0x55" instruction, a breakpoint is triggered on the
host.


Former-commit-id: 959249df4374327d90b2503d7a45f8d5d27995a6

src/bin/main.rs

@@ -60,17 +60,7 @@ fn run_emulator(matches: &ArgMatches) -> GBAResult<()> {
 
     let mut core = Core::new();
     if skip_bios {
-        core.gpr[13] = 0x0300_7f00;
+        core.skip_bios();
-        core.gpr_banked_r13[0] = 0x0300_7f00; // USR/SYS
-        core.gpr_banked_r13[1] = 0x0300_7f00; // FIQ
-        core.gpr_banked_r13[2] = 0x0300_7fa0; // IRQ
-        core.gpr_banked_r13[3] = 0x0300_7fe0; // SVC
-        core.gpr_banked_r13[4] = 0x0300_7f00; // ABT
-        core.gpr_banked_r13[5] = 0x0300_7f00; // UND
-
-        core.pc = 0x0800_0000;
-
-        core.cpsr.set(0x5f);
     }
 
     let mut gba = GameBoyAdvance::new(core, bios_bin, cart, backend);

src/core/arm7tdmi/arm/exec.rs

@@ -19,12 +19,15 @@ impl Core {
             ArmFormat::BX => self.exec_bx(bus, insn),
             ArmFormat::B_BL => self.exec_b_bl(bus, insn),
             ArmFormat::DP => self.exec_data_processing(bus, insn),
-            ArmFormat::SWI => self.exec_swi(bus),
+            ArmFormat::SWI => {
+                self.software_interrupt(bus, insn.pc + 4, insn.swi_comment());
+                Ok(())
+            }
             ArmFormat::LDR_STR => self.exec_ldr_str(bus, insn),
             ArmFormat::LDR_STR_HS_IMM => self.exec_ldr_str_hs(bus, insn),
             ArmFormat::LDR_STR_HS_REG => self.exec_ldr_str_hs(bus, insn),
             ArmFormat::LDM_STM => self.exec_ldm_stm(bus, insn),
-            ArmFormat::MRS => self.exec_mrs(bus, insn),
+            ArmFormat::MRS => self.move_from_status_register(bus, insn.rd(), insn.spsr_flag()),
             ArmFormat::MSR_REG => self.exec_msr_reg(bus, insn),
             ArmFormat::MSR_FLAGS => self.exec_msr_flags(bus, insn),
             ArmFormat::MUL_MLA => self.exec_mul_mla(bus, insn),
@@ -70,40 +73,56 @@ impl Core {
         self.branch_exchange(sb, self.get_reg(insn.rn()))
     }
 
-    fn exec_mrs(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuExecResult {
+    fn move_from_status_register(
-        let mode = self.cpsr.mode();
+        &mut self,
-        let result = if insn.spsr_flag() {
+        sb: &mut SysBus,
-            if let Some(index) = mode.spsr_index() {
+        rd: usize,
-                self.spsr[index].get()
+        is_spsr: bool,
-            } else {
+    ) -> CpuExecResult {
-                panic!("tried to get spsr from invalid mode {}", mode)
+        let result = if is_spsr {
-            }
+            self.spsr.get()
         } else {
             self.cpsr.get()
         };
-        self.set_reg(insn.rd(), result);
+        self.set_reg(rd, result);
         self.S_cycle32(sb, self.pc);
         Ok(())
     }
 
     fn exec_msr_reg(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuExecResult {
-        self.exec_msr(sb, insn, self.get_reg(insn.rm()))
+        self.write_status_register(sb, insn.spsr_flag(), self.get_reg(insn.rm()))
     }
 
-    fn exec_msr(&mut self, sb: &mut SysBus, insn: ArmInstruction, value: u32) -> CpuExecResult {
+    fn write_status_register(
-        let new_psr = RegPSR::new(value);
+        &mut self,
+        sb: &mut SysBus,
+        is_spsr: bool,
+        value: u32,
+    ) -> CpuExecResult {
+        let new_status_reg = RegPSR::new(value);
+        match self.cpsr.mode() {
+            CpuMode::User => {
+                if is_spsr {
+                    panic!("User mode can't access SPSR")
+                }
+                self.cpsr.set_flag_bits(value);
+            }
+            _ => {
+                if is_spsr {
+                    self.spsr.set(value);
+                } else {
+                    let t_bit = self.cpsr.state();
                     let old_mode = self.cpsr.mode();
-        if insn.spsr_flag() {
+                    self.cpsr.set(value);
-            if let Some(index) = old_mode.spsr_index() {
+                    if t_bit != self.cpsr.state() {
-                self.spsr[index] = new_psr;
+                        panic!("T bit changed from MSR");
-            } else {
+                    }
-                panic!("tried to change spsr from invalid mode {}", old_mode)
+                    let new_mode = new_status_reg.mode();
+                    if old_mode != new_mode {
+                        self.change_mode(old_mode, new_mode);
+                    }
                 }
-        } else {
-            if old_mode != new_psr.mode() {
-                self.change_mode(new_psr.mode());
             }
-            self.cpsr = new_psr;
         }
         self.S_cycle32(sb, self.pc);
         Ok(())
@@ -112,22 +131,18 @@ impl Core {
     fn exec_msr_flags(&mut self, sb: &mut SysBus, insn: ArmInstruction) -> CpuExecResult {
         self.S_cycle32(sb, self.pc);
         let op = insn.operand2()?;
-        let op = self.decode_operand2(op, false)?;
+        let op = self.decode_operand2(op)?;
 
         let old_mode = self.cpsr.mode();
         if insn.spsr_flag() {
-            if let Some(index) = old_mode.spsr_index() {
+            self.spsr.set_flag_bits(op);
-                self.spsr[index].set_flag_bits(op);
-            } else {
-                panic!("tried to change spsr from invalid mode {}", old_mode)
-            }
         } else {
             self.cpsr.set_flag_bits(op);
         }
         Ok(())
     }
 
-    fn decode_operand2(&mut self, op2: BarrelShifterValue, set_flags: bool) -> CpuResult<u32> {
+    fn decode_operand2(&mut self, op2: BarrelShifterValue) -> CpuResult<u32> {
         match op2 {
             BarrelShifterValue::RotatedImmediate(val, amount) => {
                 let result = self.ror(val, amount, self.cpsr.C(), false, true);
@@ -135,24 +150,18 @@ impl Core {
             }
             BarrelShifterValue::ShiftedRegister(x) => {
                 let result = self.register_shift(x)?;
-                if set_flags {
+                Ok(result)
-                    self.cpsr.set_C(self.bs_carry_out);
-                }
-                Ok(result as u32)
             }
             _ => unreachable!(),
         }
     }
 
     fn transfer_spsr_mode(&mut self) {
-        let old_mode = self.cpsr.mode();
+        let spsr = self.spsr;
-        if let Some(index) = old_mode.spsr_index() {
+        if self.cpsr.mode() != spsr.mode() {
-            let new_psr = self.spsr[index];
+            self.change_mode(self.cpsr.mode(), spsr.mode());
-            if old_mode != new_psr.mode() {
-                self.change_mode(new_psr.mode());
-            }
-            self.cpsr = new_psr;
         }
+        self.cpsr = spsr;
     }
 
     /// Logical/Arithmetic ALU operations
@@ -160,6 +169,8 @@ 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) -> CpuExecResult {
+        use AluOpCode::*;
+
         self.S_cycle32(sb, self.pc);
         let mut op1 = if insn.rn() == REG_PC {
             (insn.pc + 8) as i32
@@ -167,7 +178,7 @@ impl Core {
             self.get_reg(insn.rn()) as i32
         };
 
-        let s_flag = insn.set_cond_flag();
+        let mut s_flag = insn.set_cond_flag();
         let opcode = insn.opcode().unwrap();
 
         let op2 = insn.operand2()?;
@@ -179,21 +190,28 @@ impl Core {
             }
             _ => {}
         }
-        let op2 = self.decode_operand2(op2, s_flag)? as i32;
+        let op2 = self.decode_operand2(op2)? as i32;
 
+        let reg_rd = insn.rd();
         if !s_flag {
             match opcode {
-                AluOpCode::TEQ | AluOpCode::CMN => {
+                TEQ => {
-                    return self.exec_msr(sb, insn, op2 as u32);
+                    return self.write_status_register(sb, false, op2 as u32);
                 }
-                AluOpCode::TST | AluOpCode::CMP => return self.exec_mrs(sb, insn),
+                CMN => {
+                    return self.write_status_register(sb, true, op2 as u32);
+                }
+                TST => return self.move_from_status_register(sb, reg_rd, false),
+                CMP => return self.move_from_status_register(sb, reg_rd, true),
                 _ => (),
             }
         }
 
-        let rd = insn.rd();
+        if reg_rd == REG_PC && s_flag {
+            self.transfer_spsr_mode();
+            s_flag = false;
+        }
 
-        use AluOpCode::*;
         let C = self.cpsr.C() as i32;
         let alu_res = if s_flag {
             let mut carry = self.bs_carry_out;
@@ -239,11 +257,10 @@ impl Core {
         };
 
         if let Some(result) = alu_res {
-            if rd == REG_PC {
+            if reg_rd == REG_PC {
-                self.transfer_spsr_mode();
                 self.flush_pipeline(sb);
             }
-            self.set_reg(rd, result as u32);
+            self.set_reg(reg_rd, result as u32);
         }
 
         Ok(())

src/core/arm7tdmi/cpu.rs

@@ -30,18 +30,21 @@ pub struct Core {
     pub pc: u32,
     pub gpr: [u32; 15],
     // r13 and r14 are banked for all modes. System&User mode share them
-    pub gpr_banked_r13: [u32; 6],
+    pub(super) gpr_banked_r13: [u32; 6],
-    pub gpr_banked_r14: [u32; 6],
+    pub(super) gpr_banked_r14: [u32; 6],
     // r8-r12 are banked for fiq mode
-    pub gpr_banked_old_r8_12: [u32; 5],
+    pub(super) gpr_banked_old_r8_12: [u32; 5],
-    pub gpr_banked_fiq_r8_12: [u32; 5],
+    pub(super) gpr_banked_fiq_r8_12: [u32; 5],
 
     pub cpsr: RegPSR,
-    pub spsr: [RegPSR; 5],
+    pub(super) spsr: RegPSR,
+    pub(super) spsr_bank: [RegPSR; 6],
 
-    pub bs_carry_out: bool,
+    pub(super) bs_carry_out: bool,
 
     pipeline_state: PipelineState,
+    pipeline: [u32; 2],
+
     fetched_arm: u32,
     decoded_arm: u32,
     fetched_thumb: u16,
@@ -63,8 +66,10 @@ pub type CpuExecResult = CpuResult<()>;
 
 impl Core {
     pub fn new() -> Core {
+        let mut cpsr = RegPSR::new(0x0000_00D3);
         Core {
             memreq: 0xffff_0000, // set memreq to an invalid addr so the first load cycle will be non-sequential
+            cpsr: cpsr,
             ..Default::default()
         }
     }
@@ -125,20 +130,20 @@ impl Core {
         }
     }
 
-    pub fn write_32(&mut self, addr: Addr, value: u32, bus: &mut SysBus) {
+    pub(super) fn write_32(&mut self, addr: Addr, value: u32, bus: &mut SysBus) {
         bus.write_32(addr & !0x3, value);
     }
 
-    pub fn write_16(&mut self, addr: Addr, value: u16, bus: &mut SysBus) {
+    pub(super) fn write_16(&mut self, addr: Addr, value: u16, bus: &mut SysBus) {
         bus.write_16(addr & !0x1, value);
     }
 
-    pub fn write_8(&mut self, addr: Addr, value: u8, bus: &mut SysBus) {
+    pub(super) fn write_8(&mut self, addr: Addr, value: u8, bus: &mut SysBus) {
         bus.write_8(addr, value);
     }
 
     /// Helper function for "ldr" instruction that handles misaligned addresses
-    pub fn ldr_word(&mut self, addr: Addr, bus: &SysBus) -> u32 {
+    pub(super) fn ldr_word(&mut self, addr: Addr, bus: &SysBus) -> u32 {
         if addr & 0x3 != 0 {
             let rotation = (addr & 0x3) << 3;
             let value = bus.read_32(addr & !0x3);
@@ -149,7 +154,7 @@ impl Core {
     }
 
     /// Helper function for "ldrh" instruction that handles misaligned addresses
-    pub fn ldr_half(&mut self, addr: Addr, bus: &SysBus) -> u32 {
+    pub(super) fn ldr_half(&mut self, addr: Addr, bus: &SysBus) -> u32 {
         if addr & 0x1 != 0 {
             let rotation = (addr & 0x1) << 3;
             let value = bus.read_16(addr & !0x1);
@@ -160,7 +165,7 @@ impl Core {
     }
 
     /// Helper function for "ldrsh" instruction that handles misaligned addresses
-    pub fn ldr_sign_half(&mut self, addr: Addr, bus: &SysBus) -> u32 {
+    pub(super) fn ldr_sign_half(&mut self, addr: Addr, bus: &SysBus) -> u32 {
         if addr & 0x1 != 0 {
             bus.read_8(addr) as i8 as i32 as u32
         } else {
@@ -172,43 +177,39 @@ impl Core {
         self.gpr.clone()
     }
 
-    fn map_banked_registers(&mut self, curr_mode: CpuMode, new_mode: CpuMode) {
+    pub(super) fn change_mode(&mut self, old_mode: CpuMode, new_mode: CpuMode) {
-        let next_index = new_mode.bank_index();
+        let new_index = new_mode.bank_index();
-        let curr_index = curr_mode.bank_index();
+        let old_index = old_mode.bank_index();
 
-        self.gpr_banked_r13[curr_index] = self.gpr[13];
+        if new_index == old_index {
-        self.gpr_banked_r14[curr_index] = self.gpr[14];
+            return;
+        }
 
-        self.gpr[13] = self.gpr_banked_r13[next_index];
+        self.spsr_bank[old_index] = self.spsr;
-        self.gpr[14] = self.gpr_banked_r14[next_index];
+        self.gpr_banked_r13[old_index] = self.gpr[13];
+        self.gpr_banked_r14[old_index] = self.gpr[14];
+
+        self.spsr = self.spsr_bank[new_index];
+        self.gpr[13] = self.gpr_banked_r13[new_index];
+        self.gpr[14] = self.gpr_banked_r14[new_index];
 
         if new_mode == CpuMode::Fiq {
             for r in 0..5 {
                 self.gpr_banked_old_r8_12[r] = self.gpr[r + 8];
                 self.gpr[r + 8] = self.gpr_banked_fiq_r8_12[r];
             }
-        } else if curr_mode == CpuMode::Fiq {
+        } else if old_mode == CpuMode::Fiq {
             for r in 0..5 {
                 self.gpr_banked_fiq_r8_12[r] = self.gpr[r + 8];
                 self.gpr[r + 8] = self.gpr_banked_old_r8_12[r];
             }
         }
-    }
+        self.cpsr.set_mode(new_mode);
-
-    pub fn change_mode(&mut self, new_mode: CpuMode) {
-        let curr_mode = self.cpsr.mode();
-        // Copy CPSR to SPSR_mode
-        if let Some(index) = new_mode.spsr_index() {
-            self.spsr[index] = self.cpsr;
-        }
-        self.map_banked_registers(curr_mode, new_mode);
-        // let next_index = new_mode.bank_index();
-        // self.gpr_banked_r14[next_index] = self.get_next_pc();
     }
 
     /// Resets the cpu
     pub fn reset(&mut self, sb: &mut SysBus) {
-        self.exception(sb, Exception::Reset);
+        self.exception(sb, Exception::Reset, 0);
     }
 
     pub fn word_size(&self) -> usize {
@@ -222,18 +223,18 @@ impl Core {
         self.cycles
     }
 
-    pub fn add_cycle(&mut self) {
+    pub(super) fn add_cycle(&mut self) {
         // println!("<cycle I-Cyclel> total: {}", self.cycles);
         self.cycles += 1;
     }
 
-    pub fn add_cycles(&mut self, addr: Addr, bus: &SysBus, access: MemoryAccess) {
+    pub(super) fn add_cycles(&mut self, addr: Addr, bus: &SysBus, access: MemoryAccess) {
         let cycles_to_add = 1 + bus.get_cycles(addr, access);
         // println!("<cycle {:#x} {}> took: {}", addr, access, cycles_to_add);
         self.cycles += cycles_to_add;
     }
 
-    pub fn cycle_type(&self, addr: Addr) -> MemoryAccessType {
+    pub(super) fn cycle_type(&self, addr: Addr) -> MemoryAccessType {
         if addr == self.memreq || addr == self.memreq.wrapping_add(self.word_size() as Addr) {
             Seq
         } else {
@@ -241,7 +242,7 @@ impl Core {
         }
     }
 
-    pub fn get_required_multipiler_array_cycles(&self, rs: i32) -> usize {
+    pub(super) fn get_required_multipiler_array_cycles(&self, rs: i32) -> usize {
         if rs & 0xff == rs {
             1
         } else if rs & 0xffff == rs {
@@ -254,42 +255,42 @@ impl Core {
     }
 
     #[allow(non_snake_case)]
-    pub fn S_cycle32(&mut self, sb: &SysBus, addr: u32) {
+    pub(super) fn S_cycle32(&mut self, sb: &SysBus, addr: u32) {
         self.cycles += 1;
         self.cycles += sb.get_cycles(addr, Seq + MemoryAccess32);
     }
 
     #[allow(non_snake_case)]
-    pub fn S_cycle16(&mut self, sb: &SysBus, addr: u32) {
+    pub(super) fn S_cycle16(&mut self, sb: &SysBus, addr: u32) {
         self.cycles += 1;
         self.cycles += sb.get_cycles(addr, Seq + MemoryAccess16);
     }
 
     #[allow(non_snake_case)]
-    pub fn S_cycle8(&mut self, sb: &SysBus, addr: u32) {
+    pub(super) fn S_cycle8(&mut self, sb: &SysBus, addr: u32) {
         self.cycles += 1;
         self.cycles += sb.get_cycles(addr, Seq + MemoryAccess8);
     }
 
     #[allow(non_snake_case)]
-    pub fn N_cycle32(&mut self, sb: &SysBus, addr: u32) {
+    pub(super) fn N_cycle32(&mut self, sb: &SysBus, addr: u32) {
         self.cycles += 1;
         self.cycles += sb.get_cycles(addr, NonSeq + MemoryAccess32);
     }
 
     #[allow(non_snake_case)]
-    pub fn N_cycle16(&mut self, sb: &SysBus, addr: u32) {
+    pub(super) fn N_cycle16(&mut self, sb: &SysBus, addr: u32) {
         self.cycles += 1;
         self.cycles += sb.get_cycles(addr, NonSeq + MemoryAccess16);
     }
 
     #[allow(non_snake_case)]
-    pub fn N_cycle8(&mut self, sb: &SysBus, addr: u32) {
+    pub(super) fn N_cycle8(&mut self, sb: &SysBus, addr: u32) {
         self.cycles += 1;
         self.cycles += sb.get_cycles(addr, NonSeq + MemoryAccess8);
     }
 
-    pub fn check_arm_cond(&self, cond: ArmCond) -> bool {
+    pub(super) fn check_arm_cond(&self, cond: ArmCond) -> bool {
         use ArmCond::*;
         match cond {
             EQ => self.cpsr.Z(),
@@ -310,13 +311,8 @@ impl Core {
         }
     }
 
-    pub fn exec_swi(&mut self, sb: &mut SysBus) -> CpuExecResult {
+    pub(super) fn did_pipeline_flush(&self) -> bool {
-        match self.cpsr.state() {
+        self.pipeline_state == PipelineState::Refill1
-            CpuState::ARM => self.N_cycle32(sb, self.pc),
-            CpuState::THUMB => self.N_cycle16(sb, self.pc),
-        };
-        self.exception(sb, Exception::SoftwareInterrupt);
-        Ok(())
     }
 
     fn step_arm_exec(&mut self, insn: u32, sb: &mut SysBus) -> CpuResult<()> {
@@ -333,48 +329,30 @@ impl Core {
                 self.last_executed = None;
             }
             PipelineState::Execute => {
-                let insn = ArmInstruction::decode(insn, self.pc.wrapping_sub(8))?;
+                let decoded_arm = ArmInstruction::decode(insn, self.pc.wrapping_sub(8))?;
                 self.gpr_previous = self.get_registers();
-                self.exec_arm(sb, insn)?;
+                self.exec_arm(sb, decoded_arm)?;
                 if !self.did_pipeline_flush() {
                     self.pc = pc.wrapping_add(4);
                 }
-                self.last_executed = Some(DecodedInstruction::Arm(insn));
+                self.last_executed = Some(DecodedInstruction::Arm(decoded_arm));
             }
         }
         Ok(())
     }
 
-    fn arm(&mut self, sb: &mut SysBus) -> CpuResult<()> {
-        let pc = self.pc;
-
-        // fetch
-        let fetched_now = sb.read_32(pc);
-        let executed_now = self.decoded_arm;
-
-        // decode
-        self.decoded_arm = self.fetched_arm;
-        self.fetched_arm = fetched_now;
-
-        // execute
-        self.step_arm_exec(executed_now, sb)?;
-        Ok(())
-    }
-
-    pub fn did_pipeline_flush(&self) -> bool {
-        self.pipeline_state == PipelineState::Refill1
-    }
-
     fn step_thumb_exec(&mut self, insn: u16, sb: &mut SysBus) -> CpuResult<()> {
         let pc = self.pc;
         match self.pipeline_state {
             PipelineState::Refill1 => {
                 self.pc = pc.wrapping_add(2);
                 self.pipeline_state = PipelineState::Refill2;
+                self.last_executed = None;
             }
             PipelineState::Refill2 => {
                 self.pc = pc.wrapping_add(2);
                 self.pipeline_state = PipelineState::Execute;
+                self.last_executed = None;
             }
             PipelineState::Execute => {
                 let insn = ThumbInstruction::decode(insn, self.pc.wrapping_sub(4))?;
@@ -389,23 +367,7 @@ impl Core {
         Ok(())
     }
 
-    fn thumb(&mut self, sb: &mut SysBus) -> CpuResult<()> {
+    pub(super) fn flush_pipeline(&mut self, sb: &mut SysBus) {
-        let pc = self.pc;
-
-        // fetch
-        let fetched_now = sb.read_16(pc);
-        let executed_now = self.decoded_thumb;
-
-        // decode
-        self.decoded_thumb = self.fetched_thumb;
-        self.fetched_thumb = fetched_now;
-
-        // execute
-        self.step_thumb_exec(executed_now, sb)?;
-        Ok(())
-    }
-
-    pub fn flush_pipeline(&mut self, sb: &mut SysBus) {
         self.pipeline_state = PipelineState::Refill1;
         match self.cpsr.state() {
             CpuState::ARM => {
@@ -422,9 +384,20 @@ impl Core {
     /// Perform a pipeline step
     /// If an instruction was executed in this step, return it.
     pub fn step(&mut self, bus: &mut SysBus) -> CpuResult<()> {
+        let pc = self.pc;
+
+        let fetched_now = match self.cpsr.state() {
+            CpuState::ARM => bus.read_32(pc),
+            CpuState::THUMB => bus.read_16(pc) as u32,
+        };
+
+        let insn = self.pipeline[0];
+        self.pipeline[0] = self.pipeline[1];
+        self.pipeline[1] = fetched_now;
+
         match self.cpsr.state() {
-            CpuState::ARM => self.arm(bus),
+            CpuState::ARM => self.step_arm_exec(insn, bus),
-            CpuState::THUMB => self.thumb(bus),
+            CpuState::THUMB => self.step_thumb_exec(insn as u16, bus),
         }
     }
 
@@ -454,6 +427,25 @@ impl Core {
             }
         }
     }
+
+    pub fn get_cpu_state(&self) -> CpuState {
+        self.cpsr.state()
+    }
+
+    pub fn skip_bios(&mut self) {
+        self.gpr_banked_r13[0] = 0x0300_7f00; // USR/SYS
+        self.gpr_banked_r13[1] = 0x0300_7f00; // FIQ
+        self.gpr_banked_r13[2] = 0x0300_7fa0; // IRQ
+        self.gpr_banked_r13[3] = 0x0300_7fe0; // SVC
+        self.gpr_banked_r13[4] = 0x0300_7f00; // ABT
+        self.gpr_banked_r13[5] = 0x0300_7f00; // UND
+
+        self.gpr[13] = 0x0300_7f00;
+        self.gpr[14] = 0x0800_0000;
+        self.pc = 0x0800_0000;
+
+        self.cpsr.set(0x5f);
+    }
 }
 
 impl fmt::Display for Core {

src/core/arm7tdmi/exception.rs

@@ -18,24 +18,19 @@ pub enum Exception {
     Fiq = 0x1c,
 }
 
-impl From<Exception> for CpuMode {
-    /// Return cpu mode upon entry
-    fn from(e: Exception) -> CpuMode {
-        use Exception::*;
-        match e {
-            Reset | SoftwareInterrupt | Reserved => CpuMode::Supervisor,
-            PrefatchAbort | DataAbort => CpuMode::Abort,
-            UndefinedInstruction => CpuMode::Undefined,
-            Irq => CpuMode::Irq,
-            Fiq => CpuMode::Fiq,
-        }
-    }
-}
-
 impl Core {
-    pub fn exception(&mut self, sb: &mut SysBus, e: Exception) {
+    pub fn exception(&mut self, sb: &mut SysBus, e: Exception, lr: u32) {
-        let vector = e as u32;
+        use Exception::*;
-        let new_mode = CpuMode::from(e);
+        let (new_mode, irq_disable, fiq_disable) = match e {
+            Reset => (CpuMode::Supervisor, true, true),
+            UndefinedInstruction => (CpuMode::Undefined, false, false),
+            SoftwareInterrupt => (CpuMode::Supervisor, true, false),
+            DataAbort => (CpuMode::Abort, false, false),
+            PrefatchAbort => (CpuMode::Abort, false, false),
+            Reserved => panic!("Cpu reserved exception"),
+            Irq => (CpuMode::Irq, true, false),
+            Fiq => (CpuMode::Fiq, true, true),
+        };
         if self.verbose {
             println!(
                 "{}: {:?}, pc: {:#x}, new_mode: {:?} old_mode: {:?}",
@@ -47,19 +42,46 @@ impl Core {
             );
         }
 
-        self.change_mode(new_mode);
+        let new_bank = new_mode.bank_index();
-        self.gpr[REG_LR] = self.get_next_pc() + (self.word_size() as u32);
+        self.spsr_bank[new_bank] = self.cpsr;
+        self.gpr_banked_r14[new_bank] = lr;
+        self.change_mode(self.cpsr.mode(), new_mode);
 
         // Set appropriate CPSR bits
         self.cpsr.set_state(CpuState::ARM);
         self.cpsr.set_mode(new_mode);
+        if irq_disable {
             self.cpsr.set_irq_disabled(true);
-        if e == Exception::Reset || e == Exception::Fiq {
+        }
+        if fiq_disable {
             self.cpsr.set_fiq_disabled(true);
         }
 
         // Set PC to vector address
-        self.pc = vector;
+        self.pc = e as u32;
         self.flush_pipeline(sb);
     }
+
+    pub fn irq(&mut self, sb: &mut SysBus) {
+        if !self.cpsr.irq_disabled() {
+            let lr = self.get_next_pc() + 4;
+            self.exception(sb, Exception::Irq, lr)
+        }
+    }
+
+    pub fn software_interrupt(&mut self, sb: &mut SysBus, lr: u32, cmt: u32) {
+        match self.cpsr.state() {
+            CpuState::ARM => self.N_cycle32(sb, self.pc),
+            CpuState::THUMB => self.N_cycle16(sb, self.pc),
+        };
+        if cmt == 0x55 {
+            #[cfg(debug_assertions)]
+            {
+                println!("Special breakpoint detected!");
+                host_breakpoint!();
+            }
+        } else {
+            self.exception(sb, Exception::SoftwareInterrupt, lr);
+        }
+    }
 }

src/core/arm7tdmi/psr.rs

@@ -27,7 +27,7 @@ impl From<bool> for CpuState {
     }
 }
 
-#[derive(Debug, Clone, Copy)]
+#[derive(Debug, Clone, Copy, Default)]
 pub struct RegPSR {
     raw: u32,
 }
@@ -37,13 +37,6 @@ fn clear_reserved(n: u32) -> u32 {
     n & !RESERVED_BIT_MASK
 }
 
-impl Default for RegPSR {
-    fn default() -> RegPSR {
-        let mut psr = RegPSR::new(0);
-        psr.set_mode(CpuMode::Supervisor);
-        psr
-    }
-}
 impl RegPSR {
     pub const FLAG_BITMASK: u32 = 0xf000_0000;
 

src/core/arm7tdmi/thumb/exec.rs

@@ -562,7 +562,10 @@ impl Core {
             ThumbFormat::PushPop => self.exec_thumb_push_pop(bus, insn),
             ThumbFormat::LdmStm => self.exec_thumb_ldm_stm(bus, insn),
             ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),
-            ThumbFormat::Swi => self.exec_swi(bus),
+            ThumbFormat::Swi => {
+                self.software_interrupt(bus, insn.pc + 2, (insn.raw & 0xff) as u32);
+                Ok(())
+            }
             ThumbFormat::Branch => self.exec_thumb_branch(bus, insn),
             ThumbFormat::BranchLongWithLink => self.exec_thumb_branch_long_with_link(bus, insn),
         }

src/core/gba.rs

@@ -1,5 +1,5 @@
 /// Struct containing everything
-use super::arm7tdmi::{exception::Exception, Core, DecodedInstruction};
+use super::arm7tdmi::{exception::Exception, Core, CpuState, DecodedInstruction};
 use super::cartridge::Cartridge;
 use super::gpu::*;
 use super::interrupt::*;
@@ -64,6 +64,9 @@ impl GameBoyAdvance {
         };
 
         if !io.dmac.perform_work(&mut self.sysbus, &mut irqs) {
+            if io.intc.irq_pending() {
+                self.cpu.irq(&mut self.sysbus);
+            }
             self.cpu.step(&mut self.sysbus).unwrap();
         }
 
@@ -81,11 +84,6 @@ impl GameBoyAdvance {
             }
         }
 
-        if !self.cpu.cpsr.irq_disabled() {
         io.intc.request_irqs(irqs);
-            if io.intc.irq_pending() {
-                self.cpu.exception(&mut self.sysbus, Exception::Irq);
-            }
-        }
     }
 }

src/core/interrupt.rs

@@ -33,14 +33,11 @@ impl InterruptController {
     }
 
     pub fn request_irqs(&mut self, flags: IrqBitmask) {
-        if !self.interrupt_master_enable {
+        self.interrupt_flags.0 |= flags.0;
-            return;
-        }
-        self.interrupt_flags.0 |= flags.0 & self.interrupt_enable.0;
     }
 
     pub fn irq_pending(&self) -> bool {
-        self.interrupt_master_enable & (self.interrupt_flags.0 != 0)
+        self.interrupt_master_enable & ((self.interrupt_flags.0 & self.interrupt_enable.0) != 0)
     }
 }
 

src/debugger/command.rs

@@ -377,7 +377,7 @@ impl Debugger {
             "d" | "disass" => {
                 let (addr, n) = self.get_disassembler_args(args)?;
 
-                let m = match self.gba.cpu.cpsr.state() {
+                let m = match self.gba.cpu.get_cpu_state() {
                     CpuState::ARM => DisassMode::ModeArm,
                     CpuState::THUMB => DisassMode::ModeThumb,
                 };

src/lib.rs

@@ -1,3 +1,6 @@
+#![feature(asm)]
+#![feature(core_intrinsics)]
+
 #[macro_use]
 extern crate enum_primitive_derive;
 extern crate num;

src/util.rs

@@ -18,3 +18,12 @@ macro_rules! index2d {
         (($w as $t) * ($y as $t) + ($x as $t)) as $t
     };
 }
+
+macro_rules! host_breakpoint {
+    () => {
+        #[cfg(debug_assertions)]
+        unsafe {
+            ::std::intrinsics::breakpoint()
+        }
+    };
+}