diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs index 8a73b01..737dbb1 100644 --- a/src/arm7tdmi/thumb/display.rs +++ b/src/arm7tdmi/thumb/display.rs @@ -6,7 +6,7 @@ use super::*; use crate::arm7tdmi::reg_string; impl ThumbInstruction { - fn fmt_move_shifted_reg(&self, f: &mut fmt::Formatter) -> fmt::Result { + fn fmt_thumb_move_shifted_reg(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, "{op}\t{Rd}, {Rs}, #{Offset5}", @@ -17,7 +17,7 @@ impl ThumbInstruction { ) } - fn fmt_data_process_imm(&self, f: &mut fmt::Formatter) -> fmt::Result { + fn fmt_thumb_data_process_imm(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, "{op}\t{Rd}, #{Offset8:#x}", @@ -27,7 +27,7 @@ impl ThumbInstruction { ) } - fn fmt_high_reg_op_or_bx(&self, f: &mut fmt::Formatter) -> fmt::Result { + fn fmt_thumb_high_reg_op_or_bx(&self, f: &mut fmt::Formatter) -> fmt::Result { let op = self.format5_op(); let dst_reg = if self.flag(ThumbInstruction::FLAG_H1) { self.rd() + 8 @@ -52,7 +52,7 @@ impl ThumbInstruction { } } - fn fmt_ldr_pc(&self, f: &mut fmt::Formatter) -> fmt::Result { + fn fmt_thumb_ldr_pc(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, "ldr\t{Rd}, [pc, #{Imm:#x}] ; = #{effective:#x}", @@ -62,7 +62,7 @@ impl ThumbInstruction { ) } - fn fmt_ldr_str_reg_offset(&self, f: &mut fmt::Formatter) -> fmt::Result { + fn fmt_thumb_ldr_str_reg_offset(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, "{op}{b}\t{Rd}, [{Rb}, {Ro}]", @@ -74,7 +74,7 @@ impl ThumbInstruction { ) } - fn fmt_add_sub(&self, f: &mut fmt::Formatter) -> fmt::Result { + 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)) } else { @@ -91,7 +91,30 @@ impl ThumbInstruction { ) } - fn fmt_branch_with_cond(&self, f: &mut fmt::Formatter) -> fmt::Result { + fn fmt_thumb_push_pop(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "{}\t{{", if self.is_load() { "pop" } else { "push" })?; + let mut register_list = self.register_list().into_iter(); + let mut has_reg = false; + if let Some(reg) = register_list.next() { + write!(f, "{}", reg_string(reg))?; + has_reg = true; + } + for reg in register_list { + has_reg = true; + write!(f, ", {}", reg_string(reg))?; + }; + if self.flag(ThumbInstruction::FLAG_R) { + let r = if self.is_load() { "pc" } else { "lr" }; + if has_reg { + write!(f, ", {}", r)?; + } else { + write!(f, "{}", r)?; + } + } + write!(f, "}}") + } + + fn fmt_thumb_branch_with_cond(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, "b{cond}\t{addr:#x}", @@ -107,13 +130,14 @@ impl ThumbInstruction { impl fmt::Display for ThumbInstruction { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { match self.fmt { - ThumbFormat::MoveShiftedReg => self.fmt_move_shifted_reg(f), - ThumbFormat::AddSub => self.fmt_add_sub(f), - ThumbFormat::DataProcessImm => self.fmt_data_process_imm(f), - ThumbFormat::HiRegOpOrBranchExchange => self.fmt_high_reg_op_or_bx(f), - ThumbFormat::LdrPc => self.fmt_ldr_pc(f), - ThumbFormat::LdrStrRegOffset => self.fmt_ldr_str_reg_offset(f), - ThumbFormat::BranchConditional => self.fmt_branch_with_cond(f), + ThumbFormat::MoveShiftedReg => self.fmt_thumb_move_shifted_reg(f), + ThumbFormat::AddSub => self.fmt_thumb_add_sub(f), + ThumbFormat::DataProcessImm => self.fmt_thumb_data_process_imm(f), + 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::PushPop => self.fmt_thumb_push_pop(f), + ThumbFormat::BranchConditional => self.fmt_thumb_branch_with_cond(f), _ => write!(f, "({:?})", self), } } diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs index 9edcae0..9ebb521 100644 --- a/src/arm7tdmi/thumb/exec.rs +++ b/src/arm7tdmi/thumb/exec.rs @@ -1,9 +1,21 @@ use crate::arm7tdmi::arm::*; use crate::arm7tdmi::bus::{Bus, MemoryAccessType::*, MemoryAccessWidth::*}; use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction}; -use crate::arm7tdmi::{Addr, REG_PC, CpuState}; +use crate::arm7tdmi::{Addr, REG_PC, REG_LR, REG_SP, CpuState, reg_string}; use super::*; +fn push(cpu: &mut Core, bus: &mut Bus, r: usize) { + cpu.gpr[REG_SP] -= 4; + let stack_addr = cpu.gpr[REG_SP]; + println!("push reg {} to addr {:#x}", reg_string(r), stack_addr); + bus.write_32(stack_addr, cpu.get_reg(r)).expect("bus error"); + } +fn pop(cpu: &mut Core, bus: &mut Bus, r: usize) { + let stack_addr = cpu.gpr[REG_SP]; + let val = bus.read_32(stack_addr); + cpu.set_reg(r, val); + cpu.gpr[REG_SP] = stack_addr + 4; + } impl Core { fn exec_thumb_add_sub(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { @@ -166,6 +178,57 @@ impl Core { Ok(CpuPipelineAction::IncPC) } + fn exec_thumb_push_pop( + &mut self, + bus: &mut Bus, + insn: ThumbInstruction, + ) -> CpuExecResult { + // (From GBATEK) Execution Time: nS+1N+1I (POP), (n+1)S+2N+1I (POP PC), or (n-1)S+2N (PUSH). + + let is_pop = insn.is_load(); + let mut pipeline_action = CpuPipelineAction::IncPC; + + self.add_cycles(insn.pc, bus, NonSeq + MemoryAccess16); + + let pc_lr_flag = insn.flag(ThumbInstruction::FLAG_R); + let rlist = insn.register_list(); + if is_pop { + for r in rlist { + pop(self, bus, r); + } + if pc_lr_flag { + pop(self, bus, REG_PC); + pipeline_action = CpuPipelineAction::Flush; + self.add_cycles( + self.pc, + bus, + Seq + MemoryAccess32 + ); + self.add_cycles( + self.pc + (self.word_size() as u32), + bus, + NonSeq + MemoryAccess16 + ); + } + self.add_cycle(); + } else { + self.add_cycles( + self.gpr[REG_SP], + bus, + NonSeq + MemoryAccess32 + ); + if pc_lr_flag { + push(self, bus, REG_LR); + } + for r in rlist.into_iter().rev() { + push(self, bus ,r); + } + self.add_cycles(self.gpr[REG_SP], bus, NonSeq + MemoryAccess32); + } + + Ok(CpuPipelineAction::IncPC) + } + fn exec_thumb_branch_with_cond( &mut self, bus: &mut Bus, @@ -203,8 +266,8 @@ 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::PushPop => self.exec_thumb_push_pop(bus, insn), ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn), - _ => unimplemented!("thumb not implemented {:#?}", insn), } } diff --git a/src/arm7tdmi/thumb/mod.rs b/src/arm7tdmi/thumb/mod.rs index 0fb424c..305aa00 100644 --- a/src/arm7tdmi/thumb/mod.rs +++ b/src/arm7tdmi/thumb/mod.rs @@ -192,6 +192,7 @@ impl From for ArmOpCode { impl ThumbInstruction { const FLAG_H1: usize = 7; const FLAG_H2: usize = 6; + const FLAG_R: usize = 8; pub fn rd(&self) -> usize { match self.fmt { @@ -266,6 +267,17 @@ impl ThumbInstruction { pub fn flag(&self, bit: usize) -> bool { self.raw.bit(bit) } + + pub fn register_list(&self) -> Vec { + let list_bits = self.raw & 0xff; + let mut list = Vec::with_capacity(8); + for i in 0..=7 { + if (list_bits & (1 << i)) != 0 { + list.push(i) + } + } + list + } } #[cfg(test)]