Implement (psr / usr bank) transfers for LDM_STM

Former-commit-id: 140e6a6c75f65f08f645b1a0ff2ca7c065438ce4

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
+                self.transfer_spsr_mode();
-                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);
@@ -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 {
+        if psr_user_flag {
-            unimplemented!("Too tired to implement the mode enforcement");
+            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);
 

src/core/arm7tdmi/cpu.rs

@@ -71,22 +71,58 @@ impl Core {
         self.verbose = v;
     }
 
-    pub fn get_reg(&self, reg_num: usize) -> u32 {
+    pub fn get_reg(&self, r: usize) -> u32 {
-        match reg_num {
+        match r {
-            0...14 => self.gpr[reg_num],
+            0...14 => self.gpr[r],
             15 => self.pc,
             _ => panic!("invalid register"),
         }
     }
 
-    pub fn set_reg(&mut self, reg_num: usize, val: u32) {
+    pub fn get_reg_user(&mut self, r: usize) -> u32 {
-        match reg_num {
+        match r {
-            0...14 => self.gpr[reg_num] = val,
+            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()
     }