cpu: Model exceptions

src/arm7tdmi/arm/exec.rs

@@ -1,7 +1,6 @@
 use super::super::cpu::{
-    Core, CpuError, CpuExecResult, CpuInstruction, CpuPipelineAction, CpuResult,
+    Core, CpuError, CpuExecResult, CpuInstruction, CpuPipelineAction, CpuResult, CpuState
 };
-use super::super::psr::CpuState;
 use super::super::sysbus::SysBus;
 use super::{
     ArmCond, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmRegisterShift, ArmShiftType,

src/arm7tdmi/cpu.rs

@@ -1,11 +1,13 @@
 use std::convert::TryFrom;
 use std::fmt;
 
+use crate::num_traits::FromPrimitive;
 use colored::*;
 
-use super::reg_string;
 use super::arm::*;
-use super::psr::{CpuMode, CpuState, RegPSR};
+use super::exception::Exception;
+use super::psr::RegPSR;
+use super::reg_string;
 use super::sysbus::SysBus;
 
 #[derive(Debug, PartialEq)]
@@ -14,6 +16,73 @@ pub enum CpuInstruction {
     Thumb,
 }
 
+#[derive(Debug, PartialEq, Primitive, Copy, Clone)]
+#[repr(u8)]
+pub enum CpuState {
+    ARM = 0,
+    THUMB = 1,
+}
+
+impl fmt::Display for CpuState {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        use CpuState::*;
+        match self {
+            ARM => write!(f, "ARM"),
+            THUMB => write!(f, "THUMB"),
+        }
+    }
+}
+
+#[derive(Debug, Primitive, Copy, Clone, PartialEq)]
+pub enum CpuMode {
+    User = 0b10000,
+    Fiq = 0b10001,
+    Irq = 0b10010,
+    Supervisor = 0b10011,
+    Abort = 0b10111,
+    Undefined = 0b11011,
+    System = 0b11111,
+}
+
+impl CpuMode {
+    pub fn spsr_index(&self) -> Option<usize> {
+        match self {
+            CpuMode::Fiq => Some(0),
+            CpuMode::Irq => Some(1),
+            CpuMode::Supervisor => Some(2),
+            CpuMode::Abort => Some(3),
+            CpuMode::Undefined => Some(4),
+            _ => None,
+        }
+    }
+
+    pub fn bank_index(&self) -> usize {
+        match self {
+            CpuMode::User | CpuMode::System => 0,
+            CpuMode::Fiq => 1,
+            CpuMode::Irq => 2,
+            CpuMode::Supervisor => 3,
+            CpuMode::Abort => 4,
+            CpuMode::Undefined => 5,
+        }
+    }
+}
+
+impl fmt::Display for CpuMode {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        use CpuMode::*;
+        match self {
+            User => write!(f, "USR"),
+            Fiq => write!(f, "FIQ"),
+            Irq => write!(f, "IRQ"),
+            Supervisor => write!(f, "SVC"),
+            Abort => write!(f, "ABT"),
+            Undefined => write!(f, "UND"),
+            System => write!(f, "SYS"),
+        }
+    }
+}
+
 #[derive(Debug, PartialEq)]
 pub enum CpuError {
     ArmDecodeError(ArmDecodeError),
@@ -58,12 +127,21 @@ pub struct CpuModeContext {
     spsr: u32,
 }
 
-#[derive(Debug)]
+#[derive(Debug, Default)]
 pub struct Core {
     pub pc: u32,
     // r0-r7
-    gpr: [u32; 15],
+    pub gpr: [u32; 15],
+    // r13 and r14 are banked for all modes. System&User mode share them
+    pub gpr_banked_r13: [u32; 6],
+    pub gpr_banked_r14: [u32; 6],
+    // r8-r12 are banked for fiq mode
+    pub gpr_banked_old_r8_12: [u32; 5],
+    pub gpr_banked_fiq_r8_12: [u32; 5],
+
     pub cpsr: RegPSR,
+    pub spsr: [RegPSR; 5],
+
     pub verbose: bool,
 }
 
@@ -78,10 +156,7 @@ pub type CpuExecResult = CpuResult<(CpuInstruction, CpuPipelineAction)>;
 impl Core {
     pub fn new() -> Core {
         Core {
-            pc: 0,
-            gpr: [0; 15],
-            cpsr: RegPSR::new(),
-            verbose: false,
+            ..Default::default()
         }
     }
 
@@ -93,8 +168,7 @@ impl Core {
         match reg_num {
             0...14 => self.gpr[reg_num],
             15 => self.pc,
-            _ => panic!("invalid register")
-            // _ => 0x12345678 // unimplemented!("TODO banked registers"),
+            _ => panic!("invalid register"),
         }
     }
 
@@ -102,15 +176,46 @@ impl Core {
         match reg_num {
             0...14 => self.gpr[reg_num] = val,
             15 => self.pc = val,
-            _ => panic!("invalid register")
-            // _ => unimplemented!("TODO banked registers"),
+            _ => panic!("invalid register"),
         }
     }
 
+    fn map_banked_registers(&mut self, curr_mode: CpuMode, new_mode: CpuMode) {
+        let next_index = new_mode.bank_index();
+        let curr_index = curr_mode.bank_index();
+
+        self.gpr_banked_r13[curr_index] = self.gpr[13];
+        self.gpr[13] = self.gpr_banked_r13[next_index];
+
+        self.gpr_banked_r14[curr_index] = self.gpr[14];
+        self.gpr_banked_r14[next_index] = self.pc; // Store the return address in LR_mode
+        self.gpr[14] = self.gpr_banked_r14[next_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 {
+            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];
+            }
+        }
+    }
+
+    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);
+    }
+
     /// Resets the cpu
     pub fn reset(&mut self) {
-        self.pc = 0;
-        self.cpsr.set(0);
+        self.exception(Exception::Reset);
     }
 
     fn word_size(&self) -> usize {

src/arm7tdmi/exception.rs

@@ -0,0 +1,52 @@
+use super::cpu::{Core, CpuMode, CpuState};
+
+use colored::*;
+
+#[derive(Debug, Clone, Copy, PartialEq)]
+#[allow(dead_code)]
+/// Models a CPU exception, and maps to the relavnt entry in the exception vector
+pub enum Exception {
+    Reset = 0x00,
+    UndefinedInstruction = 0x04,
+    SoftwareInterrupt = 0x08,
+    PrefatchAbort = 0x0c,
+    DataAbort = 0x10,
+    Reserved = 0x14,
+    Irq = 0x18,
+    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, e: Exception) {
+        let vector = e as u32;
+        let new_mode = CpuMode::from(e);
+        if self.verbose {
+            println!("{}: {:?}, new_mode: {:?}", "Exception".cyan(), e, new_mode);
+        }
+
+        self.change_mode(new_mode);
+        // Set appropriate CPSR bits
+        self.cpsr.set_state(CpuState::ARM);
+        self.cpsr.set_mode(new_mode);
+        self.cpsr.set_irq_disabled(true);
+        let disabled_fiq = e == Exception::Reset || e == Exception::Fiq;
+        self.cpsr.set_fiq_disabled(disabled_fiq);
+
+        // Set PC to vector address
+        self.pc = vector;
+    }
+}

src/arm7tdmi/mod.rs

@@ -1,6 +1,8 @@
-mod psr;
 pub mod arm;
 pub mod cpu;
+mod psr;
+mod exception;
+
 pub use super::sysbus;
 
 pub const REG_PC: usize = 15;

src/arm7tdmi/psr.rs

@@ -4,27 +4,10 @@ use std::fmt;
 use crate::bit::BitIndex;
 use crate::num_traits::FromPrimitive;
 
+use super::cpu::{CpuMode, CpuState};
+
 use colored::*;
 
-use super::arm::ArmCond;
-
-#[derive(Debug, PartialEq, Primitive)]
-#[repr(u8)]
-pub enum CpuState {
-    ARM = 0,
-    THUMB = 1,
-}
-
-impl fmt::Display for CpuState {
-    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        use CpuState::*;
-        match self {
-            ARM => write!(f, "ARM"),
-            THUMB => write!(f, "THUMB"),
-        }
-    }
-}
-
 impl From<CpuState> for bool {
     fn from(state: CpuState) -> bool {
         match state {
@@ -44,33 +27,6 @@ impl From<bool> for CpuState {
     }
 }
 
-#[derive(Debug, Primitive)]
-#[repr(u8)]
-pub enum CpuMode {
-    User = 0b10000,
-    Fiq = 0b10001,
-    Irq = 0b10010,
-    Supervisor = 0b10011,
-    Abort = 0b10111,
-    Undefined = 0b11011,
-    System = 0b11111,
-}
-
-impl fmt::Display for CpuMode {
-    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        use CpuMode::*;
-        match self {
-            User => write!(f, "USR"),
-            Fiq => write!(f, "FIQ"),
-            Irq => write!(f, "IRQ"),
-            Supervisor => write!(f, "SVC"),
-            Abort => write!(f, "ABT"),
-            Undefined => write!(f, "UND"),
-            System => write!(f, "SYS"),
-        }
-    }
-}
-
 #[derive(Debug, Clone, Copy)]
 pub struct RegPSR {
     raw: u32,
@@ -81,19 +37,17 @@ fn clear_reserved(n: u32) -> u32 {
     n & !RESERVED_BIT_MASK
 }
 
-impl RegPSR {
-    pub fn new() -> RegPSR {
-        let mut psr = RegPSR { raw: 0 };
-
-        psr.set_irq_disabled(true);
-        psr.set_fiq_disabled(true);
+impl Default for RegPSR {
+    fn default() -> RegPSR {
+        let mut psr = RegPSR {
+            raw: clear_reserved(0),
+        };
         psr.set_mode(CpuMode::Supervisor);
-        psr.set_state(CpuState::ARM);
-        println!("RAW: 0x{:08x}", psr.raw);
-
         psr
     }
+}
 
+impl RegPSR {
     pub fn get(&self) -> u32 {
         self.raw
     }

src/main.rs

@@ -93,6 +93,7 @@ fn run_debug(matches: &ArgMatches) -> GBAResult<()> {
 
     let sysbus = SysBus::new(bios_bin);
     let mut core = cpu::Core::new();
+    core.reset();
     core.set_verbose(true);
     let mut debugger = Debugger::new(core, sysbus);