From 600bebc9d2587b7ee170f5c39f800844a205785d Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 5 Jul 2019 18:38:20 +0300
Subject: [PATCH] Partially Impl Arm LDM_STM (give me a break)

Manually testing seems to work, too lazy to write any tests now,
Will do it after all instructions are complete, buggy or not.


Former-commit-id: 0f36c81d6451c706408dd3c4815bfb3abea1ff44
---
 src/arm7tdmi/arm/display.rs | 10 +++++-
 src/arm7tdmi/arm/exec.rs    | 68 ++++++++++++++++++++++++++++++++++++-
 src/arm7tdmi/arm/mod.rs     |  2 +-
 src/arm7tdmi/thumb/exec.rs  |  6 +---
 src/arm7tdmi/thumb/mod.rs   |  2 +-
 5 files changed, 79 insertions(+), 9 deletions(-)

diff --git a/src/arm7tdmi/arm/display.rs b/src/arm7tdmi/arm/display.rs
index 4926206..0a7fc16 100644
--- a/src/arm7tdmi/arm/display.rs
+++ b/src/arm7tdmi/arm/display.rs
@@ -254,7 +254,15 @@ impl ArmInstruction {
         for reg in register_list {
             write!(f, ", {}", reg_string(reg))?;
         }
-        write!(f, "}}")
+        write!(
+            f,
+            "}}{}",
+            if self.psr_and_force_user_flag() {
+                "^"
+            } else {
+                ""
+            }
+        )
     }
 
     /// MRS - transfer PSR contents to a register
diff --git a/src/arm7tdmi/arm/exec.rs b/src/arm7tdmi/arm/exec.rs
index ada05c6..99a9180 100644
--- a/src/arm7tdmi/arm/exec.rs
+++ b/src/arm7tdmi/arm/exec.rs
@@ -21,6 +21,7 @@ impl Core {
             ArmFormat::DP => self.exec_data_processing(bus, insn),
             ArmFormat::SWI => self.exec_swi(bus, insn),
             ArmFormat::LDR_STR => self.exec_ldr_str(bus, insn),
+            ArmFormat::LDM_STM => self.exec_ldm_stm(bus, insn),
             ArmFormat::MSR_REG => self.exec_msr_reg(bus, insn),
             _ => Err(CpuError::UnimplementedCpuInstruction(
                 insn.pc,
@@ -115,7 +116,7 @@ impl Core {
             ArmShiftType::ROR => {
                 let amount = amount % 32;
                 let result = val.rotate_right(amount);
-                self.cpsr.set_C((result >> 1)  &1 == 1);
+                self.cpsr.set_C((result >> 1) & 1 == 1);
                 result
             }
         }
@@ -322,4 +323,69 @@ impl Core {
 
         Ok(pipeline_action)
     }
+
+    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 is_load = insn.load_flag();
+        let mut writeback = insn.write_back_flag();
+        let mut pipeline_action = CpuPipelineAction::IncPC;
+        let rn = insn.rn();
+        let mut addr = self.gpr[rn] as i32;
+
+        let step: i32 = if ascending { 4 } else { -4 };
+        let rlist = if ascending {
+            insn.register_list()
+        } else {
+            let mut rlist = insn.register_list();
+            rlist.reverse();
+            rlist
+        };
+
+        if psr_user {
+            unimplemented!("Too tired to implement the mode enforcement");
+        }
+
+        if is_load {
+            if rlist.contains(&rn) {
+                writeback = false;
+            }
+            for r in rlist {
+                if full {
+                    addr = addr.wrapping_add(step);
+                }
+
+                self.add_cycle();
+                let val = self.load_32(addr as Addr, bus);
+                self.set_reg(r, val);
+
+                if r == REG_PC {
+                    pipeline_action = CpuPipelineAction::Flush;
+                }
+
+                if !full {
+                    addr = addr.wrapping_add(step);
+                }
+            }
+        } else {
+            for r in rlist {
+                if full {
+                    addr = addr.wrapping_add(step);
+                }
+
+                self.store_32(addr as Addr, self.get_reg(r), bus);
+
+                if !full {
+                    addr = addr.wrapping_add(step);
+                }
+            }
+        }
+
+        if writeback {
+            self.set_reg(rn, addr as u32);
+        }
+
+        Ok(pipeline_action)
+    }
 }
diff --git a/src/arm7tdmi/arm/mod.rs b/src/arm7tdmi/arm/mod.rs
index 63f9e15..e8bf1f9 100644
--- a/src/arm7tdmi/arm/mod.rs
+++ b/src/arm7tdmi/arm/mod.rs
@@ -342,7 +342,7 @@ impl ArmInstruction {
         }
     }
 
-    pub fn is_loading_psr_and_forcing_user_mode(&self) -> bool {
+    pub fn psr_and_force_user_flag(&self) -> bool {
         self.raw.bit(22)
     }
 
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index f5afd66..f113d7c 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -269,11 +269,7 @@ impl Core {
         }
     }
 
-    fn exec_thumb_branch(
-        &mut self,
-        _bus: &mut Bus,
-        insn: ThumbInstruction,
-    ) -> CpuExecResult {
+    fn exec_thumb_branch(&mut self, _bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
         let offset = ((insn.offset11() << 21) >> 20) as i32;
         self.pc = (self.pc as i32).wrapping_add(offset) as u32;
         Ok(CpuPipelineAction::Flush)
diff --git a/src/arm7tdmi/thumb/mod.rs b/src/arm7tdmi/thumb/mod.rs
index 08b49f3..d1c0227 100644
--- a/src/arm7tdmi/thumb/mod.rs
+++ b/src/arm7tdmi/thumb/mod.rs
@@ -188,7 +188,7 @@ impl From<OpFormat5> for ArmOpCode {
             OpFormat5::ADD => ArmOpCode::ADD,
             OpFormat5::CMP => ArmOpCode::CMP,
             OpFormat5::MOV => ArmOpCode::MOV,
-            OpFormat5::BX => panic!("this should not be called if op = BX")
+            OpFormat5::BX => panic!("this should not be called if op = BX"),
         }
     }
 }