diff --git a/src/arm7tdmi/cpu.rs b/src/arm7tdmi/cpu.rs index 0854739..e62c85b 100644 --- a/src/arm7tdmi/cpu.rs +++ b/src/arm7tdmi/cpu.rs @@ -200,6 +200,18 @@ impl Core { } } + pub fn get_required_multipiler_array_cycles(&self, rs: i32) -> usize { + if rs & 0xff == rs { + 1 + } else if rs & 0xffff == rs { + 2 + } else if rs & 0xffffff == rs { + 3 + } else { + 4 + } + } + pub fn load_32(&mut self, addr: Addr, bus: &mut Bus) -> u32 { self.add_cycles(addr, bus, self.cycle_type(addr) + MemoryAccess32); self.memreq = addr; diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs index af17242..5b08f54 100644 --- a/src/arm7tdmi/thumb/display.rs +++ b/src/arm7tdmi/thumb/display.rs @@ -27,6 +27,25 @@ impl ThumbInstruction { ) } + fn fmt_thumb_mul(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!( + f, + "mul\t{Rd}, {Rs}", + Rd = reg_string(self.rd()), + Rs = reg_string(self.rs()) + ) + } + + fn fmt_thumb_alu_ops(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!( + f, + "{op}\t{Rd}, {Rs}", + op = self.alu_opcode(), + Rd = reg_string(self.rd()), + Rs = reg_string(self.rs()) + ) + } + 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) { @@ -147,6 +166,8 @@ impl fmt::Display for ThumbInstruction { 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::Mul => self.fmt_thumb_mul(f), + ThumbFormat::AluOps => self.fmt_thumb_alu_ops(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), diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs index bf266ec..3c1ee06 100644 --- a/src/arm7tdmi/thumb/exec.rs +++ b/src/arm7tdmi/thumb/exec.rs @@ -45,7 +45,30 @@ impl Core { ) -> CpuExecResult { let arm_alu_op: ArmOpCode = insn.format3_op().into(); let op1 = self.get_reg(insn.rd()) as i32; - let op2 = insn.offset8() as i32; + let op2 = insn.offset8() as u8 as i32; + let result = self.alu(arm_alu_op, op1, op2, true); + if let Some(result) = result { + self.set_reg(insn.rd(), result as u32); + } + + Ok(CpuPipelineAction::IncPC) + } + + fn exec_thumb_mul(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { + let op1 = self.get_reg(insn.rd()) as i32; + let op2 = self.get_reg(insn.rs()) as i32; + let m = self.get_required_multipiler_array_cycles(op2); + for i in 0..m { + self.add_cycle(); + } + self.gpr[insn.rd()] = (op1 * op2) as u32; + Ok(CpuPipelineAction::IncPC) + } + + fn exec_thumb_alu_ops(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { + let arm_alu_op: ArmOpCode = insn.alu_opcode(); + let op1 = self.get_reg(insn.rd()) as i32; + let op2 = self.get_reg(insn.rs()) as i32; let result = self.alu(arm_alu_op, op1, op2, true); if let Some(result) = result { self.set_reg(insn.rd(), result as u32); @@ -130,7 +153,7 @@ impl Core { } 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); + 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(); @@ -141,7 +164,6 @@ impl Core { 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,7 +214,7 @@ impl Core { if !self.check_arm_cond(insn.cond()) { Ok(CpuPipelineAction::IncPC) } else { - let offset = insn.offset8() as i8 as i32; + let offset = insn.offset8() as u8 as i32; self.pc = (insn.pc as i32).wrapping_add(offset) as u32; Ok(CpuPipelineAction::Flush) } @@ -202,6 +224,8 @@ impl Core { match insn.fmt { ThumbFormat::AddSub => self.exec_thumb_add_sub(bus, insn), ThumbFormat::DataProcessImm => self.exec_thumb_data_process_imm(bus, insn), + ThumbFormat::Mul => self.exec_thumb_mul(bus, insn), + ThumbFormat::AluOps => self.exec_thumb_alu_ops(bus, insn), 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), diff --git a/src/arm7tdmi/thumb/mod.rs b/src/arm7tdmi/thumb/mod.rs index 220460c..8f96df9 100644 --- a/src/arm7tdmi/thumb/mod.rs +++ b/src/arm7tdmi/thumb/mod.rs @@ -42,6 +42,8 @@ pub enum ThumbFormat { AddSub, /// Format 3 DataProcessImm, + /// Belongs to Format 4, but decoded seperatly because ArmOpcode doesn't have MUL + Mul, /// Format 4 AluOps, /// Format 5 @@ -95,6 +97,8 @@ impl InstructionDecoder for ThumbInstruction { Ok(MoveShiftedReg) } else if raw & 0xe000 == 0x2000 { Ok(DataProcessImm) + } else if raw & 0xffc0 == 0x4340 { + Ok(Mul) } else if raw & 0xfc00 == 0x4000 { Ok(AluOps) } else if raw & 0xfc00 == 0x4400 { @@ -233,6 +237,13 @@ impl ThumbInstruction { OpFormat5::from_u8(self.raw.bit_range(8..10) as u8).unwrap() } + pub fn alu_opcode(&self) -> ArmOpCode { + match self.raw.bit_range(6..10) { + 0b1101 => panic!("tried to decode MUL"), + op => ArmOpCode::from_u16(op).unwrap(), + } + } + pub fn offset5(&self) -> i8 { self.raw.bit_range(6..11) as i8 }