From 01290f6a282a7e65e0e0efdedcdec154593ff6a0 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 5 Jul 2019 13:20:19 +0300
Subject: [PATCH] Impl Thumb LdrStrSp

Untested.


Former-commit-id: 8fa842d4969e30247fc1706dfe053c7dfbb37843
---
 src/arm7tdmi/thumb/display.rs | 11 +++++++++++
 src/arm7tdmi/thumb/exec.rs    | 14 ++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs
index 5d53172..af17242 100644
--- a/src/arm7tdmi/thumb/display.rs
+++ b/src/arm7tdmi/thumb/display.rs
@@ -74,6 +74,16 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_ldr_str_sp(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "{op}\t{Rd}, [sp, #{Imm:#x}]",
+            op = if self.is_load() { "ldr" } else { "str" },
+            Rd = reg_string(self.rd()),
+            Imm = self.word8(),
+        )
+    }
+
     fn fmt_thumb_add_sub(&self, f: &mut fmt::Formatter) -> fmt::Result {
         let operand = if self.is_immediate_operand() {
             format!("#{:x}", self.raw.bit_range(6..9))
@@ -140,6 +150,7 @@ impl fmt::Display for ThumbInstruction {
             ThumbFormat::HiRegOpOrBranchExchange => self.fmt_thumb_high_reg_op_or_bx(f),
             ThumbFormat::LdrPc => self.fmt_thumb_ldr_pc(f),
             ThumbFormat::LdrStrRegOffset => self.fmt_thumb_ldr_str_reg_offset(f),
+            ThumbFormat::LdrStrSp => self.fmt_thumb_ldr_str_sp(f),
             ThumbFormat::AddSp => self.fmt_thumb_add_sp(f),
             ThumbFormat::PushPop => self.fmt_thumb_push_pop(f),
             ThumbFormat::BranchConditional => self.fmt_thumb_branch_with_cond(f),
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index a5eae37..bf266ec 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -129,6 +129,19 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
+    fn exec_thumb_ldr_str_sp(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        let addr = (self.gpr[REG_SP] & !0b10) + 4 + (insn.word8() as Addr); 
+        if insn.is_load() {
+            let data = self.load_32(addr, bus);
+            self.add_cycle();
+            self.gpr[insn.rd()] = data;
+        } else {
+            self.store_32(addr, self.gpr[insn.rd()], bus);
+        }
+        Ok(CpuPipelineAction::IncPC)
+    }
+
+
     fn exec_thumb_add_sp(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
         let op1 = self.gpr[REG_SP] as i32;
         let op2 = insn.sword7();
@@ -192,6 +205,7 @@ impl Core {
             ThumbFormat::HiRegOpOrBranchExchange => self.exec_thumb_hi_reg_op_or_bx(bus, insn),
             ThumbFormat::LdrPc => self.exec_thumb_ldr_pc(bus, insn),
             ThumbFormat::LdrStrRegOffset => self.exec_thumb_ldr_str_reg_offset(bus, insn),
+            ThumbFormat::LdrStrSp => self.exec_thumb_ldr_str_sp(bus, insn),
             ThumbFormat::AddSp => self.exec_thumb_add_sp(bus, insn),
             ThumbFormat::PushPop => self.exec_thumb_push_pop(bus, insn),
             ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),