From 2fb6f3c8840dfe73a2e8da60a3cde98de180a041 Mon Sep 17 00:00:00 2001 From: Michel Heily Date: Mon, 22 Jul 2019 09:20:29 +0300 Subject: [PATCH] Implement (psr / usr bank) transfers for LDM_STM Former-commit-id: 140e6a6c75f65f08f645b1a0ff2ca7c065438ce4 --- src/core/arm7tdmi/arm/exec.rs | 61 ++++++++++++++++++++++++++--------- src/core/arm7tdmi/cpu.rs | 48 +++++++++++++++++++++++---- 2 files changed, 87 insertions(+), 22 deletions(-) diff --git a/src/core/arm7tdmi/arm/exec.rs b/src/core/arm7tdmi/arm/exec.rs index 81dad1c..01ccd83 100644 --- a/src/core/arm7tdmi/arm/exec.rs +++ b/src/core/arm7tdmi/arm/exec.rs @@ -146,6 +146,17 @@ impl Core { } } + fn transfer_spsr_mode(&mut self) { + 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; + } + } + /// Logical/Arithmetic ALU operations /// /// Cycles: 1S+x+y (from GBATEK) @@ -179,17 +190,7 @@ impl Core { if let Some(result) = self.alu(opcode, op1, op2, set_flags) { 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.transfer_spsr_mode(); self.flush_pipeline(); } self.set_reg(rd, result as u32); @@ -325,7 +326,7 @@ impl Core { fn exec_ldm_stm(&mut self, bus: &mut Bus, insn: ArmInstruction) -> CpuExecResult { let full = insn.pre_index_flag(); let ascending = insn.add_offset_flag(); - let psr_user = insn.psr_and_force_user_flag(); + let psr_user_flag = insn.psr_and_force_user_flag(); let is_load = insn.load_flag(); let mut writeback = insn.write_back_flag(); let rn = insn.rn(); @@ -334,10 +335,27 @@ impl Core { let step: i32 = if ascending { 4 } else { -4 }; let rlist = insn.register_list(); - if psr_user { - unimplemented!("Too tired to implement the mode enforcement"); + if psr_user_flag { + match self.cpsr.mode() { + CpuMode::User | CpuMode::System => { + panic!("LDM/STM with S bit in unprivileged mode") + } + _ => {} + }; } + let user_bank_transfer = if psr_user_flag { + if is_load { + !rlist.bit(REG_PC) + } else { + true + } + } else { + false + }; + + let psr_transfer = psr_user_flag & is_load & rlist.bit(REG_PC); + if is_load { for r in 0..16 { let r = if ascending { r } else { 15 - r }; @@ -351,9 +369,16 @@ impl Core { self.add_cycle(); let val = self.load_32(addr as Addr, bus); - self.set_reg(r, val); + if user_bank_transfer { + self.set_reg_user(r, val); + } else { + self.set_reg(r, val); + } if r == REG_PC { + if psr_transfer { + self.transfer_spsr_mode(); + } self.flush_pipeline(); } @@ -373,7 +398,11 @@ impl Core { let val = if r == REG_PC { insn.pc + 12 } else { - self.get_reg(r) + if user_bank_transfer { + self.get_reg_user(r) + } else { + self.get_reg(r) + } }; self.store_32(addr as Addr, val, bus); diff --git a/src/core/arm7tdmi/cpu.rs b/src/core/arm7tdmi/cpu.rs index d318383..3906971 100644 --- a/src/core/arm7tdmi/cpu.rs +++ b/src/core/arm7tdmi/cpu.rs @@ -71,22 +71,58 @@ impl Core { self.verbose = v; } - pub fn get_reg(&self, reg_num: usize) -> u32 { - match reg_num { - 0...14 => self.gpr[reg_num], + pub fn get_reg(&self, r: usize) -> u32 { + match r { + 0...14 => self.gpr[r], 15 => self.pc, _ => panic!("invalid register"), } } - pub fn set_reg(&mut self, reg_num: usize, val: u32) { - match reg_num { - 0...14 => self.gpr[reg_num] = val, + pub fn get_reg_user(&mut self, r: usize) -> u32 { + match r { + 0..=7 => self.gpr[r], + 8..=12 => { + if self.cpsr.mode() == CpuMode::Fiq { + self.gpr[r] + } else { + self.gpr_banked_old_r8_12[r - 8] + } + } + 13 => self.gpr_banked_r13[0], + 14 => self.gpr_banked_r14[0], + _ => panic!("invalid register"), + } + } + + pub fn set_reg(&mut self, r: usize, val: u32) { + match r { + 0...14 => self.gpr[r] = val, 15 => self.pc = val & !1, _ => panic!("invalid register"), } } + pub fn set_reg_user(&mut self, r: usize, val: u32) { + match r { + 0..=7 => self.gpr[r] = val, + 8..=12 => { + if self.cpsr.mode() == CpuMode::Fiq { + self.gpr[r] = val; + } else { + self.gpr_banked_old_r8_12[r - 8] = val; + } + } + 13 => { + self.gpr_banked_r13[0] = val; + } + 14 => { + self.gpr_banked_r14[0] = val; + } + _ => panic!("invalid register"), + } + } + pub fn get_registers(&self) -> [u32; 15] { self.gpr.clone() }