diff --git a/src/arm7tdmi/arm/display.rs b/src/arm7tdmi/arm/display.rs index ba56276..aee3d1c 100644 --- a/src/arm7tdmi/arm/display.rs +++ b/src/arm7tdmi/arm/display.rs @@ -2,7 +2,7 @@ use std::fmt; use super::{AluOpCode, ArmCond, ArmFormat, ArmHalfwordTransferType, ArmInstruction}; use crate::arm7tdmi::{ - reg_string, Addr, BarrelShiftOpCode, BarrelShifterValue, ShiftedRegister, REG_PC, + psr::RegPSR, reg_string, Addr, BarrelShiftOpCode, BarrelShifterValue, ShiftedRegister, REG_PC, }; impl fmt::Display for ArmCond { @@ -117,6 +117,26 @@ impl ArmInstruction { } } + fn fmt_operand2(&self, f: &mut fmt::Formatter) -> Result, fmt::Error> { + let operand2 = self.operand2().unwrap(); + match operand2 { + BarrelShifterValue::RotatedImmediate(_, _) => { + let value = operand2.decode_rotated_immediate().unwrap(); + write!(f, "#{}\t; {:#x}", value, value)?; + Ok(Some(value as u32)) + } + BarrelShifterValue::ShiftedRegister { + reg, + shift, + added: _, + } => { + write!(f, "{}", self.make_shifted_reg_string(reg, shift))?; + Ok(None) + } + _ => panic!("invalid operand2"), + } + } + fn fmt_data_processing(&self, f: &mut fmt::Formatter) -> fmt::Result { use AluOpCode::*; @@ -149,19 +169,8 @@ impl ArmInstruction { ), }?; - let operand2 = self.operand2().unwrap(); - match operand2 { - BarrelShifterValue::RotatedImmediate(_, _) => { - let value = operand2.decode_rotated_immediate().unwrap(); - write!(f, "#{}\t; {:#x}", value, value) - } - BarrelShifterValue::ShiftedRegister { - reg, - shift, - added: _, - } => write!(f, "{}", self.make_shifted_reg_string(reg, shift)), - _ => write!(f, "RegisterNotImpl"), - } + self.fmt_operand2(f).unwrap(); + Ok(()) } fn auto_incremenet_mark(&self) -> &str { @@ -284,6 +293,27 @@ impl ArmInstruction { ) } + fn fmt_msr_flags(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!( + f, + "msr{cond}\t{psr}, ", + cond = self.cond, + psr = if self.spsr_flag() { "SPSR_f" } else { "CPSR_f" }, + )?; + if let Ok(Some(op)) = self.fmt_operand2(f) { + let psr = RegPSR::new(op & 0xf000_0000); + write!( + f, + "\t; N={} Z={} C={} V={}", + psr.N(), + psr.Z(), + psr.C(), + psr.V() + )?; + } + Ok(()) + } + fn fmt_mul_mla(&self, f: &mut fmt::Formatter) -> fmt::Result { if self.accumulate_flag() { write!( @@ -381,6 +411,7 @@ impl fmt::Display for ArmInstruction { LDM_STM => self.fmt_ldm_stm(f), MRS => self.fmt_mrs(f), MSR_REG => self.fmt_msr_reg(f), + MSR_FLAGS => self.fmt_msr_flags(f), MUL_MLA => self.fmt_mul_mla(f), MULL_MLAL => self.fmt_mull_mlal(f), LDR_STR_HS_IMM => self.fmt_ldr_str_hs(f), diff --git a/src/arm7tdmi/arm/exec.rs b/src/arm7tdmi/arm/exec.rs index b7eff45..5c5b763 100644 --- a/src/arm7tdmi/arm/exec.rs +++ b/src/arm7tdmi/arm/exec.rs @@ -24,6 +24,7 @@ impl Core { ArmFormat::LDR_STR_HS_REG => self.exec_ldr_str_hs(bus, insn), ArmFormat::LDM_STM => self.exec_ldm_stm(bus, insn), ArmFormat::MSR_REG => self.exec_msr_reg(bus, insn), + ArmFormat::MSR_FLAGS => self.exec_msr_flags(bus, insn), ArmFormat::MUL_MLA => self.exec_mul_mla(bus, insn), _ => Err(CpuError::UnimplementedCpuInstruction( insn.pc, @@ -90,15 +91,49 @@ impl Core { Ok(CpuPipelineAction::IncPC) } - /// Logical/Arithmetic ALU operations - /// - /// Cycles: 1S+x+y (from GBATEK) - /// Add x=1I cycles if Op2 shifted-by-register. Add y=1S+1N cycles if Rd=R15. - fn exec_data_processing( + fn exec_msr_flags( &mut self, _bus: &mut Bus, insn: ArmInstruction, ) -> CpuResult { + let op = insn.operand2()?; + let op = self.decode_operand2(op)?; + + let old_mode = self.cpsr.mode(); + if insn.spsr_flag() { + if let Some(index) = old_mode.spsr_index() { + self.spsr[index].set_flag_bits(op); + } else { + panic!("tried to change spsr from invalid mode {}", old_mode) + } + } else { + self.cpsr.set_flag_bits(op); + } + Ok(CpuPipelineAction::IncPC) + } + + fn decode_operand2(&mut self, op2: BarrelShifterValue) -> CpuResult { + match op2 { + BarrelShifterValue::RotatedImmediate(imm, r) => Ok(imm.rotate_right(r)), + BarrelShifterValue::ShiftedRegister { + reg, + shift, + added: _, + } => { + // +1I + self.add_cycle(); + let result = self.register_shift(reg, shift)?; + Ok(result as u32) + } + _ => unreachable!(), + } + } + + /// Logical/Arithmetic ALU operations + /// + /// Cycles: 1S+x+y (from GBATEK) + /// Add x=1I cycles if Op2 shifted-by-register. Add y=1S+1N cycles if Rd=R15. + fn exec_data_processing(&mut self, _bus: &mut Bus, insn: ArmInstruction) -> CpuExecResult { // TODO handle carry flag let mut pipeline_action = CpuPipelineAction::IncPC; @@ -109,26 +144,10 @@ impl Core { self.get_reg(insn.rn()) as i32 }; let op2 = insn.operand2()?; + let op2 = self.decode_operand2(op2)? as i32; let rd = insn.rd(); - let op2: i32 = match op2 { - BarrelShifterValue::RotatedImmediate(immediate, rotate) => { - immediate.rotate_right(rotate) as i32 - } - BarrelShifterValue::ShiftedRegister { - reg, - shift, - added: _, - } => { - // +1I - self.add_cycle(); - let result = self.register_shift(reg, shift)?; - result - } - _ => unreachable!(), - }; - let opcode = insn.opcode().unwrap(); let set_flags = opcode.is_setting_flags() || insn.set_cond_flag(); if let Some(result) = self.alu(opcode, op1, op2, set_flags) { diff --git a/src/arm7tdmi/psr.rs b/src/arm7tdmi/psr.rs index 3db2133..c0fde75 100644 --- a/src/arm7tdmi/psr.rs +++ b/src/arm7tdmi/psr.rs @@ -45,6 +45,8 @@ impl Default for RegPSR { } } impl RegPSR { + pub const FLAG_BITMASK: u32 = 0xf000_0000; + pub fn new(u: u32) -> RegPSR { RegPSR { raw: clear_reserved(u), @@ -59,6 +61,11 @@ impl RegPSR { self.raw = clear_reserved(psr); } + pub fn set_flag_bits(&mut self, value: u32) { + self.raw &= !Self::FLAG_BITMASK; + self.raw |= Self::FLAG_BITMASK & value; + } + pub fn state(&self) -> CpuState { self.raw.bit(5).into() }