diff --git a/src/cpu.rs b/src/cpu.rs index 4a23f3b..e0d07e9 100644 --- a/src/cpu.rs +++ b/src/cpu.rs @@ -163,4 +163,76 @@ impl Processor { load_res } } + + fn push_16bit_register_to_stack( + &mut self, + register: register::SixteenBit, + ) -> Result<(), Error> { + let current_sp = self + .registers + .get_single_16bit_register(register::SingleSixteenBit::StackPointer); + + // we want to pop the LSB first (i.e. we write the MSB first) + // https://rgbds.gbdev.io/docs/v0.5.2/gbz80.7#PUSH_r16 + let [lower_bits, higher_bits] = self.registers.get_16bit_register(register).to_le_bytes(); + let res = self.memory.set_both(( + ((current_sp - 1).into(), higher_bits), + ((current_sp - 2).into(), lower_bits), + )); + + if let Err(memory::Error::SetInvalidAddress(bad_addr, _)) = res { + return Err(Error::InvalidRegisterAddress( + register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), + bad_addr, + )); + } else if let Err(err) = res { + return Err(Error::Unknown(Box::new(err))); + } + + self.registers.set_16bit_register( + register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), + current_sp - 2, + ); + + Ok(()) + } + + fn pop_from_stack_to_16bit_register( + &mut self, + register: register::SixteenBit, + ) -> Result<(), Error> { + let current_sp = self + .registers + .get_single_16bit_register(register::SingleSixteenBit::StackPointer); + + let get_from_memory = |addr| { + let res = self.memory.get(addr); + + match res { + Ok(value) => Ok(value), + Err(memory::Error::GetInvalidAddress(bad_addr)) => { + Err(Error::InvalidRegisterAddress( + register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), + bad_addr, + )) + } + Err(err) => Err(Error::Unknown(Box::new(err))), + } + }; + + let popped_bytes = [ + get_from_memory(current_sp.into())?, + get_from_memory((current_sp + 1).into())?, + ]; + + let popped_value = u16::from_le_bytes(popped_bytes); + + self.registers.set_16bit_register(register, popped_value); + self.registers.set_16bit_register( + register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), + current_sp + 2, + ); + + Ok(()) + } } diff --git a/src/cpu/run/load16.rs b/src/cpu/run/load16.rs index 645bb78..cb098a8 100644 --- a/src/cpu/run/load16.rs +++ b/src/cpu/run/load16.rs @@ -47,70 +47,13 @@ impl Run for SixteenBitLoadInstruction { } SixteenBitLoadInstruction::Push { src } => { - let current_sp = processor - .registers - .get_single_16bit_register(register::SingleSixteenBit::StackPointer); - - // we want to pop the LSB first (i.e. we write the MSB first) - // https://rgbds.gbdev.io/docs/v0.5.2/gbz80.7#PUSH_r16 - let [lower_bits, higher_bits] = - processor.registers.get_combined_register(src).to_le_bytes(); - let res = processor.memory.set_both(( - ((current_sp - 1).into(), higher_bits), - ((current_sp - 2).into(), lower_bits), - )); - - if let Err(memory::Error::SetInvalidAddress(bad_addr, _)) = res { - return Err(Error::InvalidRegisterAddress( - register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), - bad_addr, - )); - } else if let Err(err) = res { - return Err(Error::Unknown(Box::new(err))); - } - - processor.registers.set_16bit_register( - register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), - current_sp - 2, - ); + processor.push_16bit_register_to_stack(src.into())?; Ok(Cycles(16)) } SixteenBitLoadInstruction::Pop { dst } => { - let current_sp = processor - .registers - .get_single_16bit_register(register::SingleSixteenBit::StackPointer); - - let get_from_memory = |addr| { - let res = processor.memory.get(addr); - - match res { - Ok(value) => Ok(value), - Err(memory::Error::GetInvalidAddress(bad_addr)) => { - Err(Error::InvalidRegisterAddress( - register::SixteenBit::Single( - register::SingleSixteenBit::StackPointer, - ), - bad_addr, - )) - } - Err(err) => Err(Error::Unknown(Box::new(err))), - } - }; - - let popped_bytes = [ - get_from_memory(current_sp.into())?, - get_from_memory((current_sp + 1).into())?, - ]; - - let popped_value = u16::from_le_bytes(popped_bytes); - - processor.registers.set_combined_register(dst, popped_value); - processor.registers.set_16bit_register( - register::SixteenBit::Single(register::SingleSixteenBit::StackPointer), - current_sp + 2, - ); + processor.pop_from_stack_to_16bit_register(dst.into())?; Ok(Cycles(12)) } diff --git a/src/cpu/run/misc.rs b/src/cpu/run/misc.rs index bcd6b63..b239fab 100644 --- a/src/cpu/run/misc.rs +++ b/src/cpu/run/misc.rs @@ -69,7 +69,7 @@ impl Run for MiscInstruction { processor.enable_interrupts(); Ok(Cycles(4)) - }, + } MiscInstruction::DisableInterrupts => { processor.disable_interrupts(); diff --git a/src/register.rs b/src/register.rs index 8ea2fc3..3fc644b 100644 --- a/src/register.rs +++ b/src/register.rs @@ -108,6 +108,18 @@ impl Default for Registers { } } +impl From for SixteenBit { + fn from(value: Combined) -> Self { + Self::Combined(value) + } +} + +impl From for SixteenBit { + fn from(value: SingleSixteenBit) -> Self { + Self::Single(value) + } +} + impl Registers { /// Get the a flag in the flag register. /// It is suggested that you use the constants in the `flag` module when calling this method. diff --git a/tests/cpu/misc.rs b/tests/cpu/misc.rs index 3e035b6..02490b3 100644 --- a/tests/cpu/misc.rs +++ b/tests/cpu/misc.rs @@ -210,8 +210,7 @@ fn test_disable_interrupts_disables_interrupts_once_enabled() { [ // Enable interrupts 0xFB, // Nop - 0x00, - 0xF3, + 0x00, 0xF3, ] .iter() .copied()