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 { 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(()) + } }