From 37117257a667f943981199ef514e42f926622acd Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 5 Jul 2019 15:50:14 +0300
Subject: [PATCH] Impl Thumb 10 (load store halfword)

Former-commit-id: 0d5e88f200613b6df2b999ecbb855ce480e73322
---
 src/arm7tdmi/thumb/display.rs | 12 ++++++++++++
 src/arm7tdmi/thumb/exec.rs    | 18 ++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs
index f0f25f8..6eb40d4 100644
--- a/src/arm7tdmi/thumb/display.rs
+++ b/src/arm7tdmi/thumb/display.rs
@@ -93,6 +93,17 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_ldr_str_halfword(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "{op}\t{Rd}, [{Rb}, #{imm:#x}]",
+            op = if self.is_load() { "ldrh" } else { "strh" },
+            Rd = reg_string(self.rd()),
+            Rb = reg_string(self.rb()),
+            imm = self.offset5() << 1
+        )
+    }
+
     fn fmt_thumb_ldr_str_sp(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
@@ -182,6 +193,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::LdrStrHalfWord => self.fmt_thumb_ldr_str_halfword(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),
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index 7d950af..fbca6d8 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -168,6 +168,23 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
+    fn exec_thumb_ldr_str_halfword(
+        &mut self,
+        bus: &mut Bus,
+        insn: ThumbInstruction,
+    ) -> CpuExecResult {
+        let base = self.gpr[insn.rb()] as i32;
+        let addr = base.wrapping_add((insn.offset5() << 1) as i32) as Addr;
+        if insn.is_load() {
+            let data = self.load_16(addr, bus);
+            self.add_cycle();
+            self.gpr[insn.rd()] = data as u32;
+        } else {
+            self.store_16(addr, self.gpr[insn.rd()] as u16, bus);
+        }
+        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() {
@@ -267,6 +284,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::LdrStrHalfWord => self.exec_thumb_ldr_str_halfword(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),