From 2864f83681710c43832d3d2b04024c99107667fe Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Wed, 10 Jul 2019 21:44:00 +0300
Subject: [PATCH] Arm: Partially implement MUL

Former-commit-id: 3b37f5cbe327e15be4cee56572c2230dbda48082
---
 src/arm7tdmi/arm/exec.rs   | 43 ++++++++++++++++++++++++++++++++++++++
 src/arm7tdmi/thumb/exec.rs |  2 +-
 2 files changed, 44 insertions(+), 1 deletion(-)

diff --git a/src/arm7tdmi/arm/exec.rs b/src/arm7tdmi/arm/exec.rs
index 797d88f..5020178 100644
--- a/src/arm7tdmi/arm/exec.rs
+++ b/src/arm7tdmi/arm/exec.rs
@@ -24,6 +24,7 @@ impl Core {
             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),
+            ArmFormat::MUL_MLA => self.exec_mul_mla(bus, insn),
             _ => Err(CpuError::UnimplementedCpuInstruction(
                 insn.pc,
                 insn.raw,
@@ -335,4 +336,46 @@ impl Core {
 
         Ok(pipeline_action)
     }
+
+    fn exec_mul_mla(
+        &mut self,
+        bus: &mut Bus,
+        insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
+        let rd = insn.rd();
+        let rn = insn.rn();
+        let rs = insn.rs();
+        let rm = insn.rm();
+
+        // check validity
+        if REG_PC == rd || REG_PC == rn || REG_PC == rs || REG_PC == rm {
+            return Err(CpuError::IllegalInstruction);
+        }
+        if rd == rm {
+            return Err(CpuError::IllegalInstruction);
+        }
+
+        if !insn.accumulate_flag() {
+            self.set_reg(insn.rn(), 0);
+        } else {
+            panic!("accumelate not implemented yet");
+        }
+
+        let op1 = self.get_reg(rm) as i32;
+        let op2 = self.get_reg(rs) as i32;
+        let result = (op1 * op2) as u32;
+        self.set_reg(rd, result);
+
+        let m = self.get_required_multipiler_array_cycles(op2);
+        for _ in 0..m {
+            self.add_cycle();
+        }
+
+        if insn.set_cond_flag() {
+            self.cpsr.set_N(result.bit(31));
+            self.cpsr.set_Z(result == 0);
+        }
+
+        Ok(CpuPipelineAction::IncPC)
+    }
 }
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index 85c608e..9dd6c55 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -369,7 +369,7 @@ impl Core {
                 self.set_reg(r, val);
             }
         } else {
-            for r in rlist.into_iter().rev() {
+            for r in rlist {
                 self.store_32(addr, self.gpr[r], bus);
                 addr += 4;
             }