Fix exceptions and dataprocess mode change

Former-commit-id: 5892131496904b621398212b9dfc077242fa9557

src/core/arm7tdmi/arm/exec.rs

@@ -180,10 +180,21 @@ impl Core {
         let rd = insn.rd();
 
         if let Some(result) = self.alu(opcode, op1, op2, set_flags) {
-            self.set_reg(rd, result as u32);
             if rd == REG_PC {
+                // TODO move this code into a function
+                let old_mode = self.cpsr.mode();
+                if let Some(index) = old_mode.spsr_index() {
+                    let new_psr = self.spsr[index];
+                    if old_mode != new_psr.mode() {
+                        self.change_mode(new_psr.mode());
+                    }
+                    self.cpsr = new_psr;
+                } else {
+                    panic!("tried to change spsr from invalid mode {}", old_mode)
+                }
                 self.flush_pipeline();
             }
+            self.set_reg(rd, result as u32);
         }
 
         Ok(())

src/core/arm7tdmi/cpu.rs

@@ -121,11 +121,8 @@ impl Core {
             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
-            .pc
-            .wrapping_sub(self.word_size() as u32)
-            .wrapping_add(4);
+        // let next_index = new_mode.bank_index();
+        // self.gpr_banked_r14[next_index] = self.get_next_pc();
     }
 
     /// Resets the cpu

src/core/arm7tdmi/exception.rs

@@ -1,5 +1,5 @@
+use super::REG_LR;
 use super::{cpu::Core, CpuMode, CpuState};
-
 use colored::*;
 
 #[derive(Debug, Clone, Copy, PartialEq)]
@@ -35,10 +35,18 @@ impl Core {
         let vector = e as u32;
         let new_mode = CpuMode::from(e);
         if self.verbose {
-            println!("{}: {:?}, new_mode: {:?}", "Exception".cyan(), e, new_mode);
+            println!(
+                "{}: {:?}, pc: {:#x}, new_mode: {:?}",
+                "Exception".cyan(),
+                e,
+                self.pc,
+                new_mode
+            );
         }
 
         self.change_mode(new_mode);
+        self.gpr[REG_LR] = self.get_next_pc() + (self.word_size() as u32);
+
         // Set appropriate CPSR bits
         self.cpsr.set_state(CpuState::ARM);
         self.cpsr.set_mode(new_mode);

src/core/arm7tdmi/thumb/display.rs

@@ -262,11 +262,7 @@ impl ThumbInstruction {
     }
 
     fn fmt_thumb_swi(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(
-            f,
-            "swi\t{value:#x}",
-            value = self.raw & 0xff,
-        )
+        write!(f, "swi\t{value:#x}", value = self.raw & 0xff,)
     }
 
     fn fmt_thumb_branch(&self, f: &mut fmt::Formatter) -> fmt::Result {