From eaf972de93bb0f37359e65cf9cd5270d7ca576a6 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Wed, 3 Jul 2019 02:15:16 +0300
Subject: [PATCH] Implement thumb format 6 (PC-Relative Load) and test it.

Former-commit-id: ae161edb0c8968913d2ef72a14053c118c6f7692
---
 src/arm7tdmi/arm/exec.rs      | 18 ++++++++++--------
 src/arm7tdmi/thumb/display.rs |  2 +-
 src/arm7tdmi/thumb/exec.rs    | 31 +++++++++++++++++++++++++------
 src/arm7tdmi/thumb/mod.rs     | 28 ++++++++++++++++++++++++++++
 4 files changed, 64 insertions(+), 15 deletions(-)

diff --git a/src/arm7tdmi/arm/exec.rs b/src/arm7tdmi/arm/exec.rs
index 701127f..04beb71 100644
--- a/src/arm7tdmi/arm/exec.rs
+++ b/src/arm7tdmi/arm/exec.rs
@@ -65,11 +65,7 @@ impl Core {
     }
 
     /// Cycles 2S+1N
-    fn exec_bx(
-        &mut self,
-        sysbus: &mut Bus,
-        insn: ArmInstruction,
-    ) -> CpuResult<CpuPipelineAction> {
+    fn exec_bx(&mut self, sysbus: &mut Bus, insn: ArmInstruction) -> CpuResult<CpuPipelineAction> {
         let rn = self.get_reg(insn.rn());
         if rn.bit(0) {
             self.cpsr.set_state(CpuState::THUMB);
@@ -162,7 +158,13 @@ impl Core {
         res
     }
 
-    pub fn alu(&mut self, opcode: ArmOpCode, op1: i32, op2: i32, set_cond_flags: bool) -> Option<i32> {
+    pub fn alu(
+        &mut self,
+        opcode: ArmOpCode,
+        op1: i32,
+        op2: i32,
+        set_cond_flags: bool,
+    ) -> Option<i32> {
         let C = self.cpsr.C() as i32;
 
         let mut carry = self.cpsr.C();
@@ -314,11 +316,11 @@ impl Core {
         if insn.load_flag() {
             let data = if insn.transfer_size() == 1 {
                 // +1N
-                self.add_cycles(dest, sysbus, NonSeq + MemoryAccess8);
+                self.add_cycles(addr, sysbus, NonSeq + MemoryAccess8);
                 sysbus.read_8(addr) as u32
             } else {
                 // +1N
-                self.add_cycles(dest, sysbus, NonSeq + MemoryAccess32);
+                self.add_cycles(addr, sysbus, NonSeq + MemoryAccess32);
                 sysbus.read_32(addr)
             };
             // +1S
diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs
index 875bece..8a73b01 100644
--- a/src/arm7tdmi/thumb/display.rs
+++ b/src/arm7tdmi/thumb/display.rs
@@ -58,7 +58,7 @@ impl ThumbInstruction {
             "ldr\t{Rd}, [pc, #{Imm:#x}] ; = #{effective:#x}",
             Rd = reg_string(self.rd()),
             Imm = self.word8(),
-            effective = self.pc + 2 + (self.word8() as Addr)
+            effective = (self.pc + 4 & !0b10) + (self.word8() as Addr)
         )
     }
 
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index d24f77f..dd82698 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -1,14 +1,14 @@
-use crate::arm7tdmi::arm::exec::*;
-use crate::arm7tdmi::arm::ArmOpCode;
+use crate::arm7tdmi::arm::*;
 use crate::arm7tdmi::bus::{Bus, MemoryAccessType::*, MemoryAccessWidth::*};
 use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction};
+use crate::arm7tdmi::{Addr, REG_PC};
 
 use super::{ThumbFormat, ThumbInstruction};
 
 impl Core {
     fn exec_thumb_data_process_imm(
         &mut self,
-        sysbus: &mut Bus,
+        bus: &mut Bus,
         insn: ThumbInstruction,
     ) -> CpuExecResult {
         let arm_alu_op: ArmOpCode = insn.format3_op().into();
@@ -21,15 +21,34 @@ impl Core {
         // +1S
         self.add_cycles(
             self.pc + (self.word_size() as u32),
-            sysbus,
+            bus,
             Seq + MemoryAccess32,
         );
         Ok(CpuPipelineAction::IncPC)
     }
 
-    pub fn exec_thumb(&mut self, sysbus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+    fn exec_thumb_ldr_pc(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        let addr = (insn.pc & !0b10) + 4 + (insn.word8() as Addr);
+        let data = bus.read_32(addr);
+        // +1N
+        self.add_cycles(addr, bus, NonSeq + MemoryAccess32);
+        // +1S
+        self.add_cycles(
+            self.pc + (self.word_size() as u32),
+            bus,
+            Seq + MemoryAccess32,
+        );
+        self.set_reg(insn.rd(), data);
+        // +1I
+        self.add_cycle();
+
+        Ok(CpuPipelineAction::IncPC)
+    }
+
+    pub fn exec_thumb(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
         match insn.fmt {
-            ThumbFormat::DataProcessImm => self.exec_thumb_data_process_imm(sysbus, insn),
+            ThumbFormat::DataProcessImm => self.exec_thumb_data_process_imm(bus, insn),
+            ThumbFormat::LdrPc => self.exec_thumb_ldr_pc(bus, insn),
             _ => unimplemented!("thumb not implemented {:#}", insn),
         }
     }
diff --git a/src/arm7tdmi/thumb/mod.rs b/src/arm7tdmi/thumb/mod.rs
index 81f45be..2404577 100644
--- a/src/arm7tdmi/thumb/mod.rs
+++ b/src/arm7tdmi/thumb/mod.rs
@@ -293,4 +293,32 @@ mod tests {
         );
         assert_eq!(core.get_reg(0), 0x27);
     }
+
+    #[test]
+    fn ldr_pc() {
+        use crate::arm7tdmi::cpu::{Core, CpuPipelineAction};
+        use crate::sysbus::BoxedMemory;
+
+        // ldr r0, [pc, #4]
+        let insn = ThumbInstruction::decode(0x4801, 0x6).unwrap();
+
+        let bytes = vec![
+            /* 0: */        0x00, 0x00,
+            /* 2: */        0x00, 0x00,
+            /* 4: */        0x00, 0x00,
+            /* 6: <pc> */   0x00, 0x00,
+            /* 8: */        0x00, 0x00, 0x00, 0x00,
+            /* c: */        0x78, 0x56, 0x34, 0x12,
+        ];
+        let mut mem = BoxedMemory::new(bytes.into_boxed_slice());
+        let mut core = Core::new();
+        core.set_reg(0, 0);
+
+        assert_eq!(format!("{}", insn), "ldr\tr0, [pc, #0x4] ; = #0xc");
+        assert_eq!(
+            core.exec_thumb(&mut mem, insn),
+            Ok(CpuPipelineAction::IncPC)
+        );
+        assert_eq!(core.get_reg(0), 0x12345678);
+    }
 }