From a605a43ef952222a8439546029bdfd76bdc9ac5c Mon Sep 17 00:00:00 2001
From: MerryMage <MerryMage@users.noreply.github.com>
Date: Mon, 18 Jul 2016 18:28:52 +0100
Subject: [PATCH] Implement Thumb Instructions: STRH (imm), LDRH (imm)

---
 src/frontend/decoder/thumb16.h                 |  4 ++--
 .../disassembler/disassembler_thumb.cpp        | 10 ++++++++++
 src/frontend/translate/translate_thumb.cpp     | 18 ++++++++++++++++++
 3 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/src/frontend/decoder/thumb16.h b/src/frontend/decoder/thumb16.h
index 18d2d369..c38f452e 100644
--- a/src/frontend/decoder/thumb16.h
+++ b/src/frontend/decoder/thumb16.h
@@ -108,8 +108,8 @@ boost::optional<const Thumb16Matcher<V>&> DecodeThumb16(u16 instruction) {
         INST(&V::thumb16_LDRB_reg,       "LDRB (reg)",               "0101110mmmnnnttt"),
         INST(&V::thumb16_LDRSH_reg,      "LDRSH (reg)",              "0101111mmmnnnttt"),
         INST(&V::thumb16_LDR_imm_t1,     "LDR (imm, T1)",            "01101vvvvvnnnttt"),
-        //INST(&V::thumb16_STRH_imm,       "STRH (imm)",               "10000vvvvvnnnttt"),
-        //INST(&V::thumb16_LDRH_imm,       "LDRH (imm)",               "10001vvvvvnnnttt"),
+        INST(&V::thumb16_STRH_imm,       "STRH (imm)",               "10000vvvvvnnnttt"),
+        INST(&V::thumb16_LDRH_imm,       "LDRH (imm)",               "10001vvvvvnnnttt"),
         //INST(&V::thumb16_STR_sp,         "STR (SP)",                 "10010dddvvvvvvvv"),
         //INST(&V::thumb16_LDR_sp,         "LDR (SP)",                 "10011dddvvvvvvvv"),
 
diff --git a/src/frontend/disassembler/disassembler_thumb.cpp b/src/frontend/disassembler/disassembler_thumb.cpp
index 6881704e..d71f27ad 100644
--- a/src/frontend/disassembler/disassembler_thumb.cpp
+++ b/src/frontend/disassembler/disassembler_thumb.cpp
@@ -264,6 +264,16 @@ public:
         return Common::StringFromFormat("ldr %s, [%s, #%u]", RegStr(t), RegStr(n), imm32);
     }
 
+    std::string thumb16_STRH_imm(Imm5 imm5, Reg n, Reg t) {
+        u32 imm32 = imm5 << 1;
+        return Common::StringFromFormat("strh %s, [%s, #%u]", RegStr(t), RegStr(n), imm32);
+    }
+
+    std::string thumb16_LDRH_imm(Imm5 imm5, Reg n, Reg t) {
+        u32 imm32 = imm5 << 1;
+        return Common::StringFromFormat("ldrh %s, [%s, #%u]", RegStr(t), RegStr(n), imm32);
+    }
+
     std::string thumb16_ADR(Reg d, Imm8 imm8) {
         u32 imm32 = imm8 << 2;
         return Common::StringFromFormat("adr %s, +#%u", RegStr(d), imm32);
diff --git a/src/frontend/translate/translate_thumb.cpp b/src/frontend/translate/translate_thumb.cpp
index cac60d15..e053027c 100644
--- a/src/frontend/translate/translate_thumb.cpp
+++ b/src/frontend/translate/translate_thumb.cpp
@@ -486,6 +486,24 @@ struct ThumbTranslatorVisitor final {
         return true;
     }
 
+    bool thumb16_STRH_imm(Imm5 imm5, Reg n, Reg t) {
+        u32 imm32 = imm5 << 1;
+        // STRH <Rt>, [<Rn>, #<imm5>]
+        auto address = ir.Add(ir.GetRegister(n), ir.Imm32(imm32));
+        auto data = ir.LeastSignificantHalf(ir.GetRegister(t));
+        ir.WriteMemory16(address, data);
+        return true;
+    }
+
+    bool thumb16_LDRH_imm(Imm5 imm5, Reg n, Reg t) {
+        u32 imm32 = imm5 << 1;
+        // LDRH <Rt>, [<Rn>, #<imm5>]
+        auto address = ir.Add(ir.GetRegister(n), ir.Imm32(imm32));
+        auto data = ir.ZeroExtendHalfToWord(ir.ReadMemory16(address));
+        ir.SetRegister(t, data);
+        return true;
+    }
+
     bool thumb16_ADR(Reg d, Imm8 imm8) {
         u32 imm32 = imm8 << 2;
         // ADR <Rd>, <label>