From d36da306183ffefe57369ce90af4df9e2a8e2378 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Sun, 7 Jul 2019 23:10:17 +0300
Subject: [PATCH] arm: Impl LDR_STR_HS

Former-commit-id: 2baa0f2fb8cf855cb4626fbb1937e17f4e7d3035
---
 src/arm7tdmi/arm/exec.rs | 76 ++++++++++++++++++++++++++++++++++++----
 1 file changed, 70 insertions(+), 6 deletions(-)

diff --git a/src/arm7tdmi/arm/exec.rs b/src/arm7tdmi/arm/exec.rs
index cc7493e..337e051 100644
--- a/src/arm7tdmi/arm/exec.rs
+++ b/src/arm7tdmi/arm/exec.rs
@@ -6,9 +6,7 @@ use crate::arm7tdmi::exception::Exception;
 use crate::arm7tdmi::psr::RegPSR;
 use crate::arm7tdmi::{Addr, CpuError, CpuResult, CpuState, DecodedInstruction, REG_PC};
 
-use super::{
-    ArmFormat, ArmInstruction, ArmOpCode, ArmRegisterShift, ArmShiftType, ArmShiftedValue,
-};
+use super::*;
 
 impl Core {
     pub fn exec_arm(&mut self, bus: &mut Bus, insn: ArmInstruction) -> CpuExecResult {
@@ -21,6 +19,8 @@ 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::LDR_STR_HS_IMM => self.exec_ldr_str_hs(bus, insn),
+            ArmFormat::LDR_STR_HS_REG => self.exec_ldr_str_hs(bus, insn),
             ArmFormat::LDM_STM => self.exec_ldm_stm(bus, insn),
             ArmFormat::MSR_REG => self.exec_msr_reg(bus, insn),
             _ => Err(CpuError::UnimplementedCpuInstruction(
@@ -241,9 +241,9 @@ impl Core {
         Ok(pipeline_action)
     }
 
-    fn get_rn_offset(&mut self, insn: &ArmInstruction) -> i32 {
+    fn get_rn_offset(&mut self, sval: ArmShiftedValue) -> i32 {
         // TODO decide if error handling or panic here
-        match insn.ldr_str_offset().unwrap() {
+        match sval {
             ArmShiftedValue::ImmediateValue(offset) => offset,
             ArmShiftedValue::ShiftedRegister {
                 reg,
@@ -284,7 +284,7 @@ impl Core {
             addr = insn.pc + 8; // prefetching
         }
 
-        let offset = self.get_rn_offset(&insn);
+        let offset = self.get_rn_offset(insn.ldr_str_offset().unwrap());
 
         let effective_addr = (addr as i32).wrapping_add(offset) as Addr;
         addr = if insn.pre_index_flag() {
@@ -328,6 +328,70 @@ impl Core {
         Ok(pipeline_action)
     }
 
+    fn exec_ldr_str_hs(
+        &mut self,
+        bus: &mut Bus,
+        insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
+        if insn.write_back_flag() && insn.rd() == insn.rn() {
+            return Err(CpuError::IllegalInstruction);
+        }
+
+        let mut pipeline_action = CpuPipelineAction::IncPC;
+
+        let mut addr = self.get_reg(insn.rn());
+        if insn.rn() == REG_PC {
+            addr = insn.pc + 8; // prefetching
+        }
+
+        let offset = self.get_rn_offset(insn.ldr_str_hs_offset().unwrap());
+
+        let effective_addr = (addr as i32).wrapping_add(offset) as Addr;
+        addr = if insn.pre_index_flag() {
+            effective_addr
+        } else {
+            addr
+        };
+
+        if insn.load_flag() {
+            let data = match insn.halfword_data_transfer_type().unwrap() {
+                ArmHalfwordTransferType::SignedByte => self.load_8(addr, bus) as u8 as i8 as u32,
+                ArmHalfwordTransferType::SignedHalfwords => {
+                    self.load_16(addr, bus) as u16 as i16 as u32
+                }
+                ArmHalfwordTransferType::UnsignedHalfwords => self.load_16(addr, bus) as u16 as u32,
+            };
+
+            self.set_reg(insn.rd(), data);
+
+            // +1I
+            self.add_cycle();
+
+            if insn.rd() == REG_PC {
+                pipeline_action = CpuPipelineAction::Flush;
+            }
+        } else {
+            let value = if insn.rd() == REG_PC {
+                insn.pc + 12
+            } else {
+                self.get_reg(insn.rd())
+            };
+
+            match insn.halfword_data_transfer_type().unwrap() {
+                ArmHalfwordTransferType::UnsignedHalfwords => {
+                    self.store_16(addr, value as u16, bus)
+                }
+                _ => panic!("invalid HS flags for L=0")
+            };
+        }
+
+        if insn.write_back_flag() {
+            self.set_reg(insn.rn(), effective_addr as u32)
+        }
+
+        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();