From 44426b5f0edd29a2a8afef3a948ca992ef738471 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Sat, 27 Jul 2019 20:20:58 +0300
Subject: [PATCH] Impl arm SWP

Also ran some rustfmt


Former-commit-id: 30480e79d7f2926d5a5f15db20427179a672a78c
---
 src/core/arm7tdmi/alu.rs         | 11 +++++++++--
 src/core/arm7tdmi/arm/display.rs | 19 ++++++++++++++++++-
 src/core/arm7tdmi/arm/exec.rs    | 18 +++++++++++++++++-
 3 files changed, 44 insertions(+), 4 deletions(-)

diff --git a/src/core/arm7tdmi/alu.rs b/src/core/arm7tdmi/alu.rs
index 710d4ed..6c0255d 100644
--- a/src/core/arm7tdmi/alu.rs
+++ b/src/core/arm7tdmi/alu.rs
@@ -167,10 +167,17 @@ impl Core {
         (((val as u32) >> 1) as i32 | (old_c << 31)) as u32
     }
 
-    pub fn ror(&mut self, val: u32, amount: u32, carry_in: bool, immediate: bool, rrx: bool) -> u32 {
+    pub fn ror(
+        &mut self,
+        val: u32,
+        amount: u32,
+        carry_in: bool,
+        immediate: bool,
+        rrx: bool,
+    ) -> u32 {
         match amount {
             0 => {
-                if immediate & rrx{
+                if immediate & rrx {
                     self.rrx(val, carry_in)
                 } else {
                     val
diff --git a/src/core/arm7tdmi/arm/display.rs b/src/core/arm7tdmi/arm/display.rs
index 2c36da5..8fe254b 100644
--- a/src/core/arm7tdmi/arm/display.rs
+++ b/src/core/arm7tdmi/arm/display.rs
@@ -356,7 +356,11 @@ impl ArmInstruction {
             f,
             "{sign}{mnem}{S}{cond}\t{RdLo}, {RdHi}, {Rm}, {Rs}",
             sign = self.sign_mark(),
-            mnem = if self.accumulate_flag() { "mlal" } else { "mull" },
+            mnem = if self.accumulate_flag() {
+                "mlal"
+            } else {
+                "mull"
+            },
             S = self.set_cond_mark(),
             cond = self.cond,
             RdLo = reg_string(self.rd_lo()),
@@ -390,6 +394,18 @@ impl ArmInstruction {
             comm = self.swi_comment()
         )
     }
+
+    fn fmt_swp(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "swp{B}{cond}\t{Rd}, {Rm}, [{Rn}]",
+            B = if self.transfer_size() == 1 { "b" } else { "" },
+            cond = self.cond,
+            Rd = reg_string(self.rd()),
+            Rm = reg_string(self.rm()),
+            Rn = reg_string(self.rn()),
+        )
+    }
 }
 
 impl fmt::Display for ArmInstruction {
@@ -409,6 +425,7 @@ impl fmt::Display for ArmInstruction {
             LDR_STR_HS_IMM => self.fmt_ldr_str_hs(f),
             LDR_STR_HS_REG => self.fmt_ldr_str_hs(f),
             SWI => self.fmt_swi(f),
+            SWP => self.fmt_swp(f),
             _ => write!(f, "({:?})", self),
         }
     }
diff --git a/src/core/arm7tdmi/arm/exec.rs b/src/core/arm7tdmi/arm/exec.rs
index 25b9891..bd5c427 100644
--- a/src/core/arm7tdmi/arm/exec.rs
+++ b/src/core/arm7tdmi/arm/exec.rs
@@ -29,6 +29,7 @@ impl Core {
             ArmFormat::MSR_FLAGS => self.exec_msr_flags(bus, insn),
             ArmFormat::MUL_MLA => self.exec_mul_mla(bus, insn),
             ArmFormat::MULL_MLAL => self.exec_mull_mlal(bus, insn),
+            ArmFormat::SWP => self.exec_arm_swp(bus, insn),
             _ => Err(CpuError::UnimplementedCpuInstruction(
                 insn.pc,
                 insn.raw,
@@ -126,7 +127,7 @@ impl Core {
     fn decode_operand2(&mut self, op2: BarrelShifterValue, set_flags: bool) -> CpuResult<u32> {
         match op2 {
             BarrelShifterValue::RotatedImmediate(val, amount) => {
-                let result = self.ror(val, amount, self.cpsr.C(), false , true);
+                let result = self.ror(val, amount, self.cpsr.C(), false, true);
                 Ok(result)
             }
             BarrelShifterValue::ShiftedRegister(x) => {
@@ -498,4 +499,19 @@ impl Core {
 
         Ok(())
     }
+
+    fn exec_arm_swp(&mut self, sb: &mut Bus, insn: ArmInstruction) -> CpuExecResult {
+        let base_addr = self.get_reg(insn.rn());
+        if insn.transfer_size() == 1 {
+            let t = self.load_8(base_addr, sb);
+            self.store_8(base_addr, self.get_reg(insn.rm()) as u8, sb);
+            self.set_reg(insn.rd(), t as u32);
+        } else {
+            let t = self.load_32(base_addr, sb);
+            self.store_32(base_addr, self.get_reg(insn.rm()), sb);
+            self.set_reg(insn.rd(), t as u32);
+        }
+
+        Ok(())
+    }
 }