diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs index dd82698..9edcae0 100644 --- a/src/arm7tdmi/thumb/exec.rs +++ b/src/arm7tdmi/thumb/exec.rs @@ -1,11 +1,37 @@ use crate::arm7tdmi::arm::*; use crate::arm7tdmi::bus::{Bus, MemoryAccessType::*, MemoryAccessWidth::*}; use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction}; -use crate::arm7tdmi::{Addr, REG_PC}; +use crate::arm7tdmi::{Addr, REG_PC, CpuState}; -use super::{ThumbFormat, ThumbInstruction}; +use super::*; impl Core { + fn exec_thumb_add_sub(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { + let op1 = self.get_reg(insn.rs()) as i32; + let op2 = if insn.is_immediate_operand() { + insn.rn() as u32 as i32 + } else { + self.get_reg(insn.rn()) as i32 + }; + let arm_alu_op = if insn.is_subtract() { + ArmOpCode::SUB + } else { + ArmOpCode::ADD + }; + + let result = self.alu(arm_alu_op, op1, op2, true); + if let Some(result) = result { + self.set_reg(insn.rd(), result as u32); + } + // +1S + self.add_cycles( + insn.pc + (self.word_size() as u32), + bus, + Seq + MemoryAccess32, + ); + Ok(CpuPipelineAction::IncPC) + } + fn exec_thumb_data_process_imm( &mut self, bus: &mut Bus, @@ -19,12 +45,60 @@ impl Core { self.set_reg(insn.rd(), result as u32); } // +1S + self.add_cycles( + insn.pc + (self.word_size() as u32), + bus, + Seq + MemoryAccess16, + ); + Ok(CpuPipelineAction::IncPC) + } + + /// Cycles 2S+1N + fn exec_thumb_bx( + &mut self, + bus: &mut Bus, + insn: ThumbInstruction, + ) -> CpuExecResult { + let src_reg = if insn.flag(ThumbInstruction::FLAG_H2) { + insn.rs() + 8 + } else { + insn.rs() + }; + + let addr = self.get_reg(src_reg); + if addr.bit(0) { + self.cpsr.set_state(CpuState::THUMB); + } else { + self.cpsr.set_state(CpuState::ARM); + } + + // +1N + self.add_cycles(self.pc, bus, NonSeq + MemoryAccess32); + + self.pc = addr & !1; + + // +2S + self.add_cycles(self.pc, bus, Seq + MemoryAccess32); self.add_cycles( self.pc + (self.word_size() as u32), bus, Seq + MemoryAccess32, ); - Ok(CpuPipelineAction::IncPC) + + Ok(CpuPipelineAction::Flush) + } + + fn exec_thumb_hi_reg_op_or_bx( + &mut self, + bus: &mut Bus, + insn: ThumbInstruction, + ) -> CpuExecResult { + if OpFormat5::BX == insn.format5_op() { + self.exec_thumb_bx(bus, insn) + } else { + unimplemented!("Sorry, I'm tired"); + Ok(CpuPipelineAction::IncPC) + } } fn exec_thumb_ldr_pc(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { @@ -34,9 +108,9 @@ impl Core { self.add_cycles(addr, bus, NonSeq + MemoryAccess32); // +1S self.add_cycles( - self.pc + (self.word_size() as u32), + insn.pc + (self.word_size() as u32), bus, - Seq + MemoryAccess32, + Seq + MemoryAccess16, ); self.set_reg(insn.rd(), data); // +1I @@ -45,11 +119,93 @@ impl Core { Ok(CpuPipelineAction::IncPC) } + fn exec_thumb_ldr_str_reg_offset( + &mut self, + bus: &mut Bus, + insn: ThumbInstruction, + ) -> CpuExecResult { + let addr = self + .get_reg(insn.rb()) + .wrapping_add(self.get_reg(insn.ro())); + if insn.is_load() { + let data = if insn.is_transfering_bytes() { + // +1N + self.add_cycles(addr, bus, NonSeq + MemoryAccess8); + bus.read_8(addr) as u32 + } else { + // +1N + self.add_cycles(addr, bus, NonSeq + MemoryAccess32); + bus.read_32(addr) + }; + + // +1S + self.add_cycles( + insn.pc + (self.word_size() as u32), + bus, + Seq + MemoryAccess32, + ); + + self.set_reg(insn.rd(), data); + + // +1I + self.add_cycle(); + } else { + self.add_cycles(addr, bus, NonSeq + MemoryAccess32); + let value = self.get_reg(insn.rd()); + if insn.is_transfering_bytes() { + // +1N + self.add_cycles(addr, bus, NonSeq + MemoryAccess8); + bus.write_8(addr, value as u8).expect("bus error"); + } else { + // +1N + self.add_cycles(addr, bus, NonSeq + MemoryAccess32); + bus.write_32(addr, value).expect("bus error"); + }; + } + + Ok(CpuPipelineAction::IncPC) + } + + fn exec_thumb_branch_with_cond( + &mut self, + bus: &mut Bus, + insn: ThumbInstruction, + ) -> CpuExecResult { + if !self.check_arm_cond(insn.cond()) { + self.add_cycles( + insn.pc + (self.word_size() as u32), + bus, + Seq + MemoryAccess16, + ); + Ok(CpuPipelineAction::IncPC) + } else { + // +1N + self.add_cycles(insn.pc, bus, NonSeq + MemoryAccess32); + let offset = insn.offset8() as i8 as i32; + self.pc = (insn.pc as i32).wrapping_add(offset) as u32; + + // +2S + self.add_cycles(self.pc, bus, Seq + MemoryAccess32); + self.add_cycles( + self.pc + (self.word_size() as u32), + bus, + Seq + MemoryAccess32, + ); + + Ok(CpuPipelineAction::Flush) + } + } + pub fn exec_thumb(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult { match insn.fmt { + ThumbFormat::AddSub => self.exec_thumb_add_sub(bus, insn), ThumbFormat::DataProcessImm => self.exec_thumb_data_process_imm(bus, insn), + ThumbFormat::HiRegOpOrBranchExchange => self.exec_thumb_hi_reg_op_or_bx(bus, insn), ThumbFormat::LdrPc => self.exec_thumb_ldr_pc(bus, insn), - _ => unimplemented!("thumb not implemented {:#}", insn), + ThumbFormat::LdrStrRegOffset => self.exec_thumb_ldr_str_reg_offset(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 2404577..0fb424c 100644 --- a/src/arm7tdmi/thumb/mod.rs +++ b/src/arm7tdmi/thumb/mod.rs @@ -151,7 +151,7 @@ impl InstructionDecoder for ThumbInstruction { } } -#[derive(Debug, Primitive)] +#[derive(Debug, Primitive, PartialEq)] pub enum OpFormat3 { MOV = 0, CMP = 1, @@ -170,7 +170,7 @@ impl From for ArmOpCode { } } -#[derive(Debug, Primitive)] +#[derive(Debug, Primitive, PartialEq)] pub enum OpFormat5 { ADD = 0, CMP = 1, @@ -321,4 +321,44 @@ mod tests { ); assert_eq!(core.get_reg(0), 0x12345678); } + + #[test] + fn ldr_str_reg_offset() { + use crate::arm7tdmi::{ + cpu::{Core, CpuPipelineAction}, + Bus, + }; + use crate::sysbus::BoxedMemory; + + // str r0, [r4, r1] + let str_insn = ThumbInstruction::decode(0x5060, 0x6).unwrap(); + // ldrb r2, [r4, r1] + let ldr_insn = ThumbInstruction::decode(0x5c62, 0x6).unwrap(); + + let mut core = Core::new(); + core.set_reg(0, 0x12345678); + core.set_reg(2, 0); + core.set_reg(1, 0x4); + core.set_reg(4, 0xc); + + let bytes = vec![ + /* 0: */ 0xaa, 0xbb, 0xcc, 0xdd, /* 4: */ 0xaa, 0xbb, 0xcc, 0xdd, + /* 8: */ 0xaa, 0xbb, 0xcc, 0xdd, /* c: */ 0xaa, 0xbb, 0xcc, 0xdd, + /* 10: */ 0xaa, 0xbb, 0xcc, 0xdd, + ]; + let mut mem = BoxedMemory::new(bytes.into_boxed_slice()); + + assert_eq!(format!("{}", str_insn), "str\tr0, [r4, r1]"); + assert_eq!(format!("{}", ldr_insn), "ldrb\tr2, [r4, r1]"); + assert_eq!( + core.exec_thumb(&mut mem, str_insn), + Ok(CpuPipelineAction::IncPC) + ); + assert_eq!(mem.read_32(0x10), 0x12345678); + assert_eq!( + core.exec_thumb(&mut mem, ldr_insn), + Ok(CpuPipelineAction::IncPC) + ); + assert_eq!(core.get_reg(2), 0x78); + } } diff --git a/src/sysbus.rs b/src/sysbus.rs index 13007ec..b257370 100644 --- a/src/sysbus.rs +++ b/src/sysbus.rs @@ -65,6 +65,46 @@ impl Bus for BoxedMemory { } } +#[derive(Debug)] +struct DummyBus([u8; 4]); + +impl Bus for DummyBus { + fn read_32(&self, addr: Addr) -> u32 { + 0 + } + + fn read_16(&self, addr: Addr) -> u16 { + 0 + } + + fn read_8(&self, addr: Addr) -> u8 { + 0 + } + + fn write_32(&mut self, addr: Addr, value: u32) -> Result<(), io::Error> { + Ok(()) + } + + fn write_16(&mut self, addr: Addr, value: u16) -> Result<(), io::Error> { + Ok(()) + } + + fn write_8(&mut self, addr: Addr, value: u8) -> Result<(), io::Error> { + Ok(()) + } + fn get_bytes(&self, addr: Addr) -> &[u8] { + &self.0 + } + + fn get_bytes_mut(&mut self, addr: Addr) -> &mut [u8] { + &mut self.0 + } + + fn get_cycles(&self, addr: Addr, access: MemoryAccess) -> usize { + 1 + } +} + #[derive(Debug)] pub struct SysBus { bios: BoxedMemory, @@ -76,6 +116,7 @@ pub struct SysBus { vram: BoxedMemory, oam: BoxedMemory, gamepak: Cartridge, + dummy: DummyBus, } impl SysBus { @@ -98,6 +139,7 @@ impl SysBus { ), oam: BoxedMemory::new(vec![0; OAM_SIZE].into_boxed_slice()), gamepak: gamepak, + dummy: DummyBus([0; 4]), } } @@ -111,7 +153,10 @@ impl SysBus { 0x0600_0000...0x0601_7fff => &self.vram, 0x0700_0000...0x0700_03ff => &self.oam, 0x0800_0000...0x09ff_ffff => &self.gamepak, - _ => panic!("unmapped address @0x{:08x}", addr), + _ => { + println!("unmapped address @0x{:08x}", addr); + &self.dummy + } } } @@ -126,7 +171,10 @@ impl SysBus { 0x0600_0000...0x0601_7fff => &mut self.vram, 0x0700_0000...0x0700_03ff => &mut self.oam, 0x0800_0000...0x09ff_ffff => &mut self.gamepak, - _ => panic!("unmapped address @0x{:08x}", addr), + _ => { + println!("unmapped address @0x{:08x}", addr); + &mut self.dummy + } } } }