From c0d437b1a1cf284e765fcc20598daf0a02f68272 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Mon, 22 Jul 2019 01:16:48 +0300
Subject: [PATCH] Fix exceptions and dataprocess mode change

Former-commit-id: 5892131496904b621398212b9dfc077242fa9557
---
 src/core/arm7tdmi/arm/exec.rs      | 13 ++++++++++++-
 src/core/arm7tdmi/cpu.rs           |  7 ++-----
 src/core/arm7tdmi/exception.rs     | 12 ++++++++++--
 src/core/arm7tdmi/thumb/display.rs |  6 +-----
 4 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/src/core/arm7tdmi/arm/exec.rs b/src/core/arm7tdmi/arm/exec.rs
index 2aae728..c44fb6d 100644
--- a/src/core/arm7tdmi/arm/exec.rs
+++ b/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(())
diff --git a/src/core/arm7tdmi/cpu.rs b/src/core/arm7tdmi/cpu.rs
index d4bc33f..d318383 100644
--- a/src/core/arm7tdmi/cpu.rs
+++ b/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
diff --git a/src/core/arm7tdmi/exception.rs b/src/core/arm7tdmi/exception.rs
index dd7a258..e1139be 100644
--- a/src/core/arm7tdmi/exception.rs
+++ b/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);
diff --git a/src/core/arm7tdmi/thumb/display.rs b/src/core/arm7tdmi/thumb/display.rs
index 2673643..3d605ea 100644
--- a/src/core/arm7tdmi/thumb/display.rs
+++ b/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 {