diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs index 5d53172..af17242 100644 --- a/src/arm7tdmi/thumb/display.rs +++ b/src/arm7tdmi/thumb/display.rs @@ -74,6 +74,16 @@ impl ThumbInstruction { ) } + fn fmt_thumb_ldr_str_sp(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!( + f, + "{op}\t{Rd}, [sp, #{Imm:#x}]", + op = if self.is_load() { "ldr" } else { "str" }, + Rd = reg_string(self.rd()), + Imm = self.word8(), + ) + } + fn fmt_thumb_add_sub(&self, f: &mut fmt::Formatter) -> fmt::Result { let operand = if self.is_immediate_operand() { format!("#{:x}", self.raw.bit_range(6..9)) @@ -140,6 +150,7 @@ impl fmt::Display for ThumbInstruction { ThumbFormat::HiRegOpOrBranchExchange => self.fmt_thumb_high_reg_op_or_bx(f), ThumbFormat::LdrPc => self.fmt_thumb_ldr_pc(f), ThumbFormat::LdrStrRegOffset => self.fmt_thumb_ldr_str_reg_offset(f), + ThumbFormat::LdrStrSp => self.fmt_thumb_ldr_str_sp(f), ThumbFormat::AddSp => self.fmt_thumb_add_sp(f), ThumbFormat::PushPop => self.fmt_thumb_push_pop(f), ThumbFormat::BranchConditional => self.fmt_thumb_branch_with_cond(f), diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs index a5eae37..bf266ec 100644 --- a/src/arm7tdmi/thumb/exec.rs +++ b/src/arm7tdmi/thumb/exec.rs @@ -129,6 +129,19 @@ impl Core { Ok(CpuPipelineAction::IncPC) } + fn exec_thumb_ldr_str_sp(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { + let addr = (self.gpr[REG_SP] & !0b10) + 4 + (insn.word8() as Addr); + if insn.is_load() { + let data = self.load_32(addr, bus); + self.add_cycle(); + self.gpr[insn.rd()] = data; + } else { + self.store_32(addr, self.gpr[insn.rd()], bus); + } + Ok(CpuPipelineAction::IncPC) + } + + fn exec_thumb_add_sp(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { let op1 = self.gpr[REG_SP] as i32; let op2 = insn.sword7(); @@ -192,6 +205,7 @@ impl Core { ThumbFormat::HiRegOpOrBranchExchange => self.exec_thumb_hi_reg_op_or_bx(bus, insn), ThumbFormat::LdrPc => self.exec_thumb_ldr_pc(bus, insn), ThumbFormat::LdrStrRegOffset => self.exec_thumb_ldr_str_reg_offset(bus, insn), + ThumbFormat::LdrStrSp => self.exec_thumb_ldr_str_sp(bus, insn), ThumbFormat::AddSp => self.exec_thumb_add_sp(bus, insn), ThumbFormat::PushPop => self.exec_thumb_push_pop(bus, insn), ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),