From d3f2af5d3bb4546b3d3a6bbcbba8405c111601e5 Mon Sep 17 00:00:00 2001 From: august Date: Fri, 2 Feb 2024 13:44:15 -0500 Subject: [PATCH] Finished first draft of instruction logic --- src/main.rs | 913 ++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 736 insertions(+), 177 deletions(-) diff --git a/src/main.rs b/src/main.rs index 044b7b5..15cffa1 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,6 +1,6 @@ -#[allow(warnings)] +#![allow(dead_code)] use core::panic; -use std::{collections::HashMap, thread::sleep, time::Duration, u16}; +use std::{collections::HashMap, ptr::addr_of, thread::sleep, time::Duration, u16}; type Byte = u8; type Word = u16; @@ -32,7 +32,10 @@ impl MemArea { fn swap_page(&mut self, page: usize) -> Result<(), MappingError> { match page > self.pages { true => Err(MappingError::InvalidPage), - false => Ok(self.page = page), + false => { + self.page = page; + Ok(()) + } } } fn translate_address(&self, address: Word) -> Word { @@ -68,14 +71,15 @@ impl Mem { return Err(MappingError::RegionOccupied); } } - Ok(self.areas.push(area)) + self.areas.push(area); + Ok(()) } fn read(&self, address: Word) -> Result { for area in &self.areas { if area.contains(address) { let translated_address = area.translate_address(address); match area.data.get(translated_address as usize) { - Some(data) => return Ok(data.clone()), + Some(data) => return Ok(*data), None => return Err(MemoryError::NoDataAtAddress), } } @@ -86,7 +90,7 @@ impl Mem { fn write(&mut self, address: Word, data: Byte) -> Result<(), MemoryError> { for area in self.areas.iter_mut() { if area.contains(address) { - println!("Writing to area {label}", label = area.label); + // println!("Writing to area {label}", label = area.label); let translated_address = area.translate_address(address); area.data[translated_address as usize] = data; return Ok(()); @@ -155,7 +159,8 @@ impl Cpu { fn reset(&mut self) -> Result<(), ExecutionError> { let reset_vector_pointer = self.read_word(0xFFFC)?; self.pending_cycles = 8; - Ok(self.pc = reset_vector_pointer) + self.pc = reset_vector_pointer; + Ok(()) } fn read(&self, address: Word) -> Result { self.memory.read(address) @@ -173,7 +178,8 @@ impl Cpu { fn push_stack(&mut self, data: Byte) -> Result<(), ExecutionError> { self.s -= 0x1; - Ok(self.write(self.stack_addr(), data)?) + self.write(self.stack_addr(), data)?; + Ok(()) } fn push_stack_word(&mut self, address: Word) -> Result<(), ExecutionError> { @@ -256,9 +262,9 @@ impl Cpu { sleep(Duration::from_nanos(500)); if self.pending_cycles == 0 { if self.nmi || (self.irq && !self.get_flag(StatusFlag::IrqDisable)) { - self.push_stack_word(self.pc); + let _ = self.push_stack_word(self.pc); self.set_flag(StatusFlag::BrkIrq, true); - self.push_stack(self.p); + let _ = self.push_stack(self.p); self.set_flag(StatusFlag::IrqDisable, true); if self.nmi { self.pc = match self.read_word(0xFFFA) { @@ -284,29 +290,35 @@ impl Cpu { let instruction = get_instruction(opcode); match instruction { Instruction::Valid(valid_instruction) => { - println!("accumulator: {a:?}, x: {x:?}, y: {y:?}, program counter: {pc:?}, stack pointer: {s:?}, status register: {p:?}, irq pin: {irq:?}, nmi pin: {nmi:?}", a = self.a, x = self.x, y = self.y, pc = self.pc, s = self.s, p = self.p, irq = self.irq, nmi = self.nmi); + println!("accumulator: {a:#04x}, x: {x:#04x}, y: {y:#04x}, program counter: {pc:#06x}, stack pointer: {s:#04x}, status register: {p:#04x}, irq pin: {irq:?}, nmi pin: {nmi:?}", a = self.a, x = self.x, y = self.y, pc = self.pc, s = self.s, p = self.p, irq = self.irq, nmi = self.nmi); match valid_instruction.opcode.call(self) { Ok(_) => { self.pending_cycles += valid_instruction.cycles as usize; } Err(error) => match error { ExecutionError::InvalidInstruction => { - panic!("An invalid instruction was called") + panic!( + "An invalid instruction was called at address {:#06x}", + self.pc + ) } ExecutionError::StackOverflow => panic!("Stack overflowed!"), ExecutionError::MemoryError(error) => match error { MemoryError::Unmapped => panic!( - "Cpu tried to access memory that hasn't yet been mapped at {:?}", + "Cpu tried to access memory that hasn't yet been mapped at {:#06x}", self.pc ), MemoryError::NoDataAtAddress => panic!( - "Cpu tried to read from address {:?} but there was no data", + "Cpu tried to read from address {:#06x} but there was no data", self.pc ), }, ExecutionError::InterruptsDisabled => panic!("InterruptsDisabled"), ExecutionError::IncompatibleAddrMode => { - panic!("An IncompatibleAddrMode") + panic!( + "An IncompatibleAddrMode was used at address {:#06x} for instruction {:?}", self.pc, + get_instruction(self.read(self.pc).unwrap()) + ) } }, } @@ -319,29 +331,32 @@ impl Cpu { } } -#[derive(Clone)] +#[derive(Clone, Debug)] enum Instruction { Valid(ValidInstruction), Invalid, } -#[derive(Clone)] +#[derive(Clone, Debug)] struct ValidInstruction { opcode: Opcode, cycles: Byte, } -#[derive(Clone)] +#[derive(Clone, Debug)] enum AddressingMode { - AbsoluteA, // a - AbsoluteIndexedWithX, // a, x - AbsoluteIndexedWithY, // a, y - AbsoluteIndirect, // (a) - AbsoluteIndexedIndirect, // (a,x) (4), only used with the JMP instruction - Accumulator, // A - Immediate, // # - Implied, // i - ProgramCounterRelative, // r + AbsoluteA, // a + AbsoluteIndexedWithX, // a, x + AbsoluteIndexedWithY, // a, y + AbsoluteIndirect, // (a) + AbsoluteIndexedIndirect, // (a,x) (4), only used with the JMP instruction + Accumulator, // A + Immediate, // # + Implied, // i + ProgramCounterRelative, // r + ProgramCounterRelativeTest, // r, but for instructions like BBR0-7, where the first operand + // is a zero-page address, and the second is the relative branch + // address Stack, // s ZeroPage, // zp ZeroPageIndexedWithX, // zp, x @@ -444,7 +459,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x0f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR0(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR0(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -544,7 +559,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x1f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR1(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR1(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -638,7 +653,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x2f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR2(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR2(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -738,7 +753,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x3f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR3(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR3(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -826,7 +841,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x4f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR4(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR4(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -914,7 +929,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x5f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR5(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR5(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1008,7 +1023,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x6f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR6(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR6(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1108,7 +1123,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x7f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBR7(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBR7(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1202,7 +1217,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x8f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS0(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS0(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1302,7 +1317,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0x9f, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS1(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS1(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1402,7 +1417,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0xaf, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS2(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS2(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1502,7 +1517,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0xbf, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS3(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS3(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1602,7 +1617,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0xcf, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS4(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS4(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1696,7 +1711,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0xdf, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS5(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS5(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1790,7 +1805,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0xef, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS6(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS6(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1878,7 +1893,7 @@ fn get_instruction(opcode: u8) -> Instruction { ( 0xff, Instruction::Valid(ValidInstruction { - opcode: Opcode::BBS7(AddressingMode::ProgramCounterRelative), + opcode: Opcode::BBS7(AddressingMode::ProgramCounterRelativeTest), cycles: 4, }), ), @@ -1886,7 +1901,7 @@ fn get_instruction(opcode: u8) -> Instruction { opcodes.get(&opcode).unwrap().clone() } -#[derive(Clone)] +#[derive(Clone, Debug)] enum Opcode { ADC(AddressingMode), AND(AddressingMode), @@ -1990,12 +2005,13 @@ enum Opcode { #[derive(Clone, Copy)] enum AddressingModeValue { - Implied, // Used when an instruction doesn't need an operation, e.g. RTS - Relative(Byte), // Used by branch instructions to offset the program counter - Absolute(Word), // Just a plain ol' address + Implied, // Used when an instruction doesn't need an operation, e.g. RTS + Relative(Byte), // Used by branch instructions to offset the program counter + RelativeTest(Byte, Byte), // Used by branch instructions to offset the program counter + Absolute(Word), // Just a plain ol' address Accumulator(Byte), // This isn't technically accurate, since the datasheet says this would be - // Implied, but we do use the value of the accumulator as the operand so this - // is how i'm gonna do it + // Implied, but we do use the value of the accumulator as the operand so this + // is how i'm gonna do it } impl TryFrom for u16 { @@ -2003,6 +2019,7 @@ impl TryFrom for u16 { fn try_from(value: AddressingModeValue) -> Result { match value { AddressingModeValue::Implied => Err(ExecutionError::IncompatibleAddrMode), + AddressingModeValue::RelativeTest(_zero_page, offset) => Ok(offset as u16), AddressingModeValue::Relative(inner_value) => Ok(inner_value as u16), AddressingModeValue::Absolute(inner_value) => Ok(inner_value), AddressingModeValue::Accumulator(inner_value) => Ok(inner_value as u16), @@ -2137,6 +2154,17 @@ fn get_address( Ok(AddressingModeValue::Relative(byte)) } + fn relative_test( + // r + cpu: &mut Cpu, + ) -> Result { + let byte: Byte = cpu.read(cpu.pc)?; + cpu.pc += 1; + let address = cpu.read(cpu.pc)?; + cpu.pc += 1; + Ok(AddressingModeValue::RelativeTest(byte, address)) + } + match mode { AddressingMode::AbsoluteA => absolute_a(cpu), AddressingMode::AbsoluteIndexedWithX => absolute_indexed_with_x(cpu), @@ -2152,15 +2180,83 @@ fn get_address( AddressingMode::ZeroPageIndexedIndirect => zero_page_indexed_indirect(cpu), AddressingMode::ZeroPageIndirectIndexedWithY => zero_page_indirect_indexed_with_y(cpu), AddressingMode::Accumulator => Ok(accumulator(cpu)), - AddressingMode::Implied => Ok(implied(cpu)), - _ => Ok(AddressingModeValue::Implied), + AddressingMode::ProgramCounterRelativeTest => relative_test(cpu), + _ => Ok(implied(cpu)), + } +} + +fn signed_byte_to_word(value: Byte) -> Word { + let mut value = u16::from(value); + if value & 0x80 > 0 { + value |= 0xff00; + } + value +} + +fn branch( + cpu: &mut Cpu, + condition: bool, + value: AddressingModeValue, +) -> Result<(), ExecutionError> { + match value { + AddressingModeValue::Relative(address) => { + let address = signed_byte_to_word(address).wrapping_add(cpu.pc); + if condition { + if address & 0xff00 != cpu.pc & 0xff00 { + cpu.pending_cycles += 2; + } else { + cpu.pending_cycles += 1; + } + cpu.pc = address; + } + Ok(()) + } + AddressingModeValue::RelativeTest(_zero_page, address) => { + let address = signed_byte_to_word(address).wrapping_add(cpu.pc); + if condition { + if address & 0xff00 != cpu.pc & 0xff00 { + cpu.pending_cycles += 2; + } else { + cpu.pending_cycles += 1; + } + cpu.pc = address; + } + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), } } impl Opcode { fn call(&self, cpu: &mut Cpu) -> Result<(), ExecutionError> { match self { - Opcode::ADC(mode) => Ok(()), + Opcode::ADC(mode) => match mode { + AddressingMode::Immediate + | AddressingMode::ZeroPage + | AddressingMode::ZeroPageIndexedWithX + | AddressingMode::AbsoluteA + | AddressingMode::AbsoluteIndexedWithX + | AddressingMode::AbsoluteIndexedWithY + | AddressingMode::ZeroPageIndirect + | AddressingMode::ZeroPageIndexedIndirect + | AddressingMode::ZeroPageIndirectIndexedWithY => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let carry = cpu.get_flag(StatusFlag::Carry); + let result = cpu.a as Word + byte as Word + carry as Word; + cpu.set_flag(StatusFlag::Carry, result > u16::from(u8::max_value())); + cpu.set_flag(StatusFlag::Zero, result as Byte == 0); + cpu.set_flag( + StatusFlag::Overflow, + (cpu.a ^ byte) & (cpu.a ^ result as Byte) & 0b1000_0000 > 0, + // Truly have no idea what's happening here, + // check out https://docs.rs/emulator_6502/latest/src/emulator_6502/opcodes/mod.rs.html + ); + cpu.set_flag(StatusFlag::Negative, cpu.is_negative(result as Byte)); + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, Opcode::AND(mode) => match mode { AddressingMode::Immediate | AddressingMode::ZeroPage @@ -2181,7 +2277,6 @@ impl Opcode { }, Opcode::ASL(mode) => { fn asl(cpu: &mut Cpu, value: Byte) -> Byte { - let carry = cpu.get_flag(StatusFlag::Carry) as Byte; cpu.set_flag(StatusFlag::Carry, value & 0b1000_0000 == 0b1000_0000); let shifted_value = value << 1; cpu.set_flag( @@ -2192,7 +2287,10 @@ impl Opcode { shifted_value } match mode { - AddressingMode::Accumulator => Ok(cpu.a = asl(cpu, cpu.a)), + AddressingMode::Accumulator => { + cpu.a = asl(cpu, cpu.a); + Ok(()) + } AddressingMode::AbsoluteA | AddressingMode::AbsoluteIndexedWithX | AddressingMode::ZeroPage @@ -2200,138 +2298,380 @@ impl Opcode { let address = get_address(mode, cpu)?; let value = cpu.read(address.try_into()?)?; let result = asl(cpu, value); - Ok(cpu.write(address.try_into()?, result)?) + cpu.write(address.try_into()?, result)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), } } - Opcode::BBR0(mode) => Ok(()), - Opcode::BBR1(mode) => Ok(()), - Opcode::BBR2(mode) => Ok(()), - Opcode::BBR3(mode) => Ok(()), - Opcode::BBR4(mode) => Ok(()), - Opcode::BBR5(mode) => Ok(()), - Opcode::BBR6(mode) => Ok(()), - Opcode::BBR7(mode) => Ok(()), - Opcode::BBS0(mode) => Ok(()), - Opcode::BBS1(mode) => Ok(()), - Opcode::BBS2(mode) => Ok(()), - Opcode::BBS3(mode) => Ok(()), - Opcode::BBS4(mode) => Ok(()), - Opcode::BBS5(mode) => Ok(()), - Opcode::BBS6(mode) => Ok(()), - Opcode::BBS7(mode) => Ok(()), + Opcode::BBR0(mode) => match mode { + // These instructions are weird, cause they're relative, except the byte being tested is immediate, + // so i'm not sure if what i've already written is going to model this accurately + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_0001 != 0b0000_0001, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR1(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_0010 != 0b0000_0010, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR2(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_0100 != 0b0000_0100, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR3(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_1000 != 0b0000_1000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR4(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0001_0000 != 0b0001_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR5(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0010_0000 != 0b0010_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR6(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0100_0000 != 0b0100_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBR7(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b1000_0000 != 0b1000_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS0(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_0001 == 0b0000_0001, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS1(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_0010 == 0b0000_0010, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS2(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_0100 == 0b0000_0100, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS3(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0000_1000 == 0b0000_1000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS4(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0001_0000 == 0b0001_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS5(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0010_0000 == 0b0010_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS6(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b0100_0000 == 0b0100_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BBS7(mode) => match mode { + AddressingMode::ProgramCounterRelativeTest => match get_address(mode, cpu) { + Ok(value) => match value { + AddressingModeValue::RelativeTest(byte, _address) => { + let zero_page_address = byte as Word; + let test_byte = cpu.read(zero_page_address)?; + branch(cpu, test_byte & 0b1000_0000 == 0b1000_0000, value)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Err(error) => Err(error), + }, + _ => Err(ExecutionError::IncompatibleAddrMode), + }, Opcode::BCC(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if !cpu.get_flag(StatusFlag::Carry) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, !cpu.get_flag(StatusFlag::Carry), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::BCS(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if cpu.get_flag(StatusFlag::Carry) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, cpu.get_flag(StatusFlag::Carry), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::BEQ(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if cpu.get_flag(StatusFlag::Zero) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, cpu.get_flag(StatusFlag::Zero), address)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BIT(mode) => match mode { + AddressingMode::Immediate + | AddressingMode::ZeroPage + | AddressingMode::ZeroPageIndexedWithX + | AddressingMode::AbsoluteA + | AddressingMode::AbsoluteIndexedWithX => { + let address = get_address(mode, cpu)?; + let result = cpu.a & cpu.read(address.try_into()?)?; + cpu.set_flag(StatusFlag::Zero, result == 0); + cpu.set_flag( + StatusFlag::Overflow, + result & StatusFlag::Overflow as Byte == StatusFlag::Overflow as Byte, + ); + cpu.set_flag( + StatusFlag::Negative, + result & StatusFlag::Negative as Byte == StatusFlag::Negative as Byte, + ); + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, - Opcode::BIT(mode) => Ok(()), Opcode::BMI(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if cpu.get_flag(StatusFlag::Negative) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, cpu.get_flag(StatusFlag::Negative), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::BNE(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if !cpu.get_flag(StatusFlag::Zero) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, !cpu.get_flag(StatusFlag::Zero), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::BPL(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if !cpu.get_flag(StatusFlag::Negative) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, !cpu.get_flag(StatusFlag::Negative), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::BRA(mode) => match mode { AddressingMode::ProgramCounterRelative => { let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) + branch(cpu, true, address)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::BRK(mode) => match mode { + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::Brk, true); + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, - Opcode::BRK(mode) => Ok(()), Opcode::BVC(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if !cpu.get_flag(StatusFlag::Overflow) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, !cpu.get_flag(StatusFlag::Overflow), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::BVS(mode) => match mode { AddressingMode::ProgramCounterRelative => { - if cpu.get_flag(StatusFlag::Overflow) { - let address = get_address(mode, cpu)?; - Ok(cpu.pc += u16::try_from(address)?) - } else { - Ok(cpu.pc += 1) - } + let address = get_address(mode, cpu)?; + branch(cpu, cpu.get_flag(StatusFlag::Overflow), address)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::CLC(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::Carry, false)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::Carry, false); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::CLD(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::Decimal, false)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::Decimal, false); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::CLI(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::IrqDisable, false)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::IrqDisable, false); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::CLV(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::Overflow, false)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::Overflow, false); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::CMP(mode) => match mode { @@ -2385,16 +2725,23 @@ impl Opcode { | AddressingMode::AbsoluteA | AddressingMode::AbsoluteIndexedWithX => { let address = get_address(mode, cpu)?; - Ok(cpu.write(address.try_into()?, cpu.read(address.try_into()?)? - 1)?) + cpu.write(address.try_into()?, cpu.read(address.try_into()?)? - 1)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::DEX(mode) => match mode { - AddressingMode::Implied => Ok(cpu.x -= 1), + AddressingMode::Implied => { + cpu.x -= 1; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::DEY(mode) => match mode { - AddressingMode::Implied => Ok(cpu.y -= 1), + AddressingMode::Implied => { + cpu.y -= 1; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::EOR(mode) => match mode { @@ -2430,16 +2777,23 @@ impl Opcode { let byte = cpu.read(address.try_into()?)?; cpu.set_flag(StatusFlag::Zero, byte + 1 == 0); cpu.set_flag(StatusFlag::Negative, cpu.is_negative(byte + 1)); - Ok(cpu.write(address.try_into()?, byte + 1)?) + cpu.write(address.try_into()?, byte + 1)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::INX(mode) => match mode { - AddressingMode::Implied => Ok(cpu.x += 1), + AddressingMode::Implied => { + cpu.x += 1; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::INY(mode) => match mode { - AddressingMode::Implied => Ok(cpu.y += 1), + AddressingMode::Implied => { + cpu.y += 1; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::JMP(mode) => match mode { @@ -2447,7 +2801,8 @@ impl Opcode { | AddressingMode::AbsoluteIndirect | AddressingMode::AbsoluteIndexedIndirect => { let address = get_address(mode, cpu)?; - Ok(cpu.pc = address.try_into()?) + cpu.pc = address.try_into()?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2456,7 +2811,8 @@ impl Opcode { let return_address = cpu.pc - 1; cpu.push_stack_word(return_address)?; let address = get_address(mode, cpu)?; - Ok(cpu.pc = address.try_into()?) + cpu.pc = address.try_into()?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2511,7 +2867,6 @@ impl Opcode { }, Opcode::LSR(mode) => { fn lsr(cpu: &mut Cpu, value: Byte) -> Byte { - let carry = cpu.get_flag(StatusFlag::Carry) as Byte; cpu.set_flag(StatusFlag::Carry, value & 1 == 1); let shifted_value = value >> 1; cpu.set_flag( @@ -2522,7 +2877,10 @@ impl Opcode { shifted_value } match mode { - AddressingMode::Accumulator => Ok(cpu.a = lsr(cpu, cpu.a)), + AddressingMode::Accumulator => { + cpu.a = lsr(cpu, cpu.a); + Ok(()) + } AddressingMode::AbsoluteA | AddressingMode::AbsoluteIndexedWithX | AddressingMode::ZeroPage @@ -2530,12 +2888,16 @@ impl Opcode { let address = get_address(mode, cpu)?; let value = cpu.read(address.try_into()?)?; let result = lsr(cpu, value); - Ok(cpu.write(address.try_into()?, result)?) + cpu.write(address.try_into()?, result)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), } } - Opcode::NOP(mode) => Ok(cpu.pc += 1), + Opcode::NOP(_mode) => { + cpu.pc += 1; + Ok(()) + } Opcode::ORA(mode) => match mode { AddressingMode::Immediate | AddressingMode::ZeroPage @@ -2555,45 +2917,141 @@ impl Opcode { _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PHA(mode) => match mode { - AddressingMode::Stack => Ok(cpu.push_stack(cpu.a)?), + AddressingMode::Stack => { + cpu.push_stack(cpu.a)?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PHP(mode) => match mode { - AddressingMode::Stack => Ok(cpu.push_stack(cpu.p)?), + AddressingMode::Stack => { + cpu.push_stack(cpu.p)?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PHX(mode) => match mode { - AddressingMode::Stack => Ok(cpu.push_stack(cpu.x)?), + AddressingMode::Stack => { + cpu.push_stack(cpu.x)?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PHY(mode) => match mode { - AddressingMode::Stack => Ok(cpu.push_stack(cpu.y)?), + AddressingMode::Stack => { + cpu.push_stack(cpu.y)?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PLA(mode) => match mode { - AddressingMode::Stack => Ok(cpu.a = cpu.pop_stack()?), + AddressingMode::Stack => { + cpu.a = cpu.pop_stack()?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PLP(mode) => match mode { - AddressingMode::Stack => Ok(cpu.p = cpu.pop_stack()?), + AddressingMode::Stack => { + cpu.p = cpu.pop_stack()?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PLX(mode) => match mode { - AddressingMode::Stack => Ok(cpu.x = cpu.pop_stack()?), + AddressingMode::Stack => { + cpu.x = cpu.pop_stack()?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::PLY(mode) => match mode { - AddressingMode::Stack => Ok(cpu.y = cpu.pop_stack()?), + AddressingMode::Stack => { + cpu.y = cpu.pop_stack()?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB0(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1111_1110; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB1(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1111_1101; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB2(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1111_1011; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB3(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1111_0111; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB4(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1110_1111; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB5(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1101_1111; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB6(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b1011_1111; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::RMB7(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b0111_1111; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, - Opcode::RMB0(mode) => Ok(()), - Opcode::RMB1(mode) => Ok(()), - Opcode::RMB2(mode) => Ok(()), - Opcode::RMB3(mode) => Ok(()), - Opcode::RMB4(mode) => Ok(()), - Opcode::RMB5(mode) => Ok(()), - Opcode::RMB6(mode) => Ok(()), - Opcode::RMB7(mode) => Ok(()), Opcode::ROL(mode) => { fn rol(cpu: &mut Cpu, value: Byte) -> Byte { let carry = cpu.get_flag(StatusFlag::Carry) as Byte; @@ -2607,7 +3065,10 @@ impl Opcode { shifted_value } match mode { - AddressingMode::Accumulator => Ok(cpu.a = rol(cpu, cpu.a)), + AddressingMode::Accumulator => { + cpu.a = rol(cpu, cpu.a); + Ok(()) + } AddressingMode::AbsoluteA | AddressingMode::AbsoluteIndexedWithX | AddressingMode::ZeroPage @@ -2615,7 +3076,8 @@ impl Opcode { let address = get_address(mode, cpu)?; let value = cpu.read(address.try_into()?)?; let result = rol(cpu, value); - Ok(cpu.write(address.try_into()?, result)?) + cpu.write(address.try_into()?, result)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), } @@ -2633,7 +3095,10 @@ impl Opcode { shifted_value } match mode { - AddressingMode::Accumulator => Ok(cpu.a = ror(cpu, cpu.a)), + AddressingMode::Accumulator => { + cpu.a = ror(cpu, cpu.a); + Ok(()) + } AddressingMode::AbsoluteA | AddressingMode::AbsoluteIndexedWithX | AddressingMode::ZeroPage @@ -2641,7 +3106,8 @@ impl Opcode { let address = get_address(mode, cpu)?; let value = cpu.read(address.try_into()?)?; let result = ror(cpu, value); - Ok(cpu.write(address.try_into()?, result)?) + cpu.write(address.try_into()?, result)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), } @@ -2657,7 +3123,8 @@ impl Opcode { Opcode::RTS(mode) => match mode { AddressingMode::Implied => { let return_address = cpu.pop_stack_word()?; - Ok(cpu.pc = return_address + 1) + cpu.pc = return_address + 1; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2689,25 +3156,106 @@ impl Opcode { _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::SEC(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::Carry, true)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::Carry, true); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::SED(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::Decimal, true)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::Decimal, true); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::SEI(mode) => match mode { - AddressingMode::Implied => Ok(cpu.set_flag(StatusFlag::IrqDisable, true)), + AddressingMode::Implied => { + cpu.set_flag(StatusFlag::IrqDisable, true); + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB0(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b0000_0001; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB1(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b0000_0010; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB2(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b0000_0100; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB3(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b0000_1000; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB4(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b0001_0000; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB5(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b0010_0000; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB6(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte & 0b0100_0000; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } + _ => Err(ExecutionError::IncompatibleAddrMode), + }, + Opcode::SMB7(mode) => match mode { + AddressingMode::ZeroPage => { + let address = get_address(mode, cpu)?; + let byte = cpu.read(address.try_into()?)?; + let reset_byte = byte | 0b1000_0000; + cpu.write(address.try_into()?, reset_byte)?; + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, - Opcode::SMB0(mode) => Ok(()), - Opcode::SMB1(mode) => Ok(()), - Opcode::SMB2(mode) => Ok(()), - Opcode::SMB3(mode) => Ok(()), - Opcode::SMB4(mode) => Ok(()), - Opcode::SMB5(mode) => Ok(()), - Opcode::SMB6(mode) => Ok(()), - Opcode::SMB7(mode) => Ok(()), Opcode::STA(mode) => match mode { AddressingMode::AbsoluteA | AddressingMode::AbsoluteIndexedWithX @@ -2718,12 +3266,16 @@ impl Opcode { | AddressingMode::ZeroPageIndexedWithX | AddressingMode::ZeroPageIndirectIndexedWithY => { let address = get_address(mode, cpu)?; - Ok(cpu.write(address.try_into()?, cpu.a)?) + cpu.write(address.try_into()?, cpu.a)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::STP(mode) => match mode { - AddressingMode::Implied => Ok(cpu.stop()), + AddressingMode::Implied => { + cpu.stop(); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::STX(mode) => match mode { @@ -2731,7 +3283,8 @@ impl Opcode { | AddressingMode::ZeroPage | AddressingMode::ZeroPageIndexedWithY => { let address = get_address(mode, cpu)?; - Ok(cpu.write(address.try_into()?, cpu.x)?) + cpu.write(address.try_into()?, cpu.x)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2740,7 +3293,8 @@ impl Opcode { | AddressingMode::ZeroPage | AddressingMode::ZeroPageIndexedWithX => { let address = get_address(mode, cpu)?; - Ok(cpu.write(address.try_into()?, cpu.y)?) + cpu.write(address.try_into()?, cpu.y)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2750,7 +3304,8 @@ impl Opcode { | AddressingMode::ZeroPage | AddressingMode::ZeroPageIndexedWithX => { let address = get_address(mode, cpu)?; - Ok(cpu.write(address.try_into()?, 0)?) + cpu.write(address.try_into()?, 0)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2789,7 +3344,8 @@ impl Opcode { AddressingMode::AbsoluteA | AddressingMode::ZeroPage => { let address = get_address(mode, cpu)?; let byte = cpu.read(address.try_into()?)?; - Ok(cpu.write(address.try_into()?, cpu.a | byte)?) + cpu.write(address.try_into()?, cpu.a | byte)?; + Ok(()) } _ => Err(ExecutionError::IncompatibleAddrMode), }, @@ -2828,7 +3384,10 @@ impl Opcode { _ => Err(ExecutionError::IncompatibleAddrMode), }, Opcode::WAI(mode) => match mode { - AddressingMode::Implied => Ok(cpu.wait_for_interrupt()), + AddressingMode::Implied => { + cpu.wait_for_interrupt(); + Ok(()) + } _ => Err(ExecutionError::IncompatibleAddrMode), }, }