1 files changed, 65 insertions, 1 deletions

diff --git a/src/cpu.rs b/src/cpu.rs
index d2d6d4d..1955a86 100644
--- a/src/cpu.rs
+++ b/src/cpu.rs
@@ -3,6 +3,7 @@
 //! Implementation of the MOS 6502 CPU.
 
 use bitflags::bitflags;
+use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt};
 use tracing::{debug, trace};
 
 use crate::{
@@ -97,6 +98,29 @@ impl Cpu {
 		data
 	}
 
+	/// Fetches the next byte, pointed to by the program counter.
+	fn fetch_word(&mut self, cycles: &mut u32, memory: &Memory) -> u16 {
+		debug!("fetching word from address {:#04X}", self.program_counter);
+
+		// MOS6502 is little endian.
+		let data: u16 = memory
+			.data
+			.get(usize::from(self.program_counter)..usize::from(self.program_counter + 2))
+			.expect("program_counter indexed outside of memory")
+			.read_u16::<LittleEndian>()
+			.expect("could not read word from memory");
+
+		debug!(
+			"fetched word {data:#04X} from address {:#04X}",
+			self.program_counter
+		);
+
+		self.program_counter = self.program_counter.wrapping_add(2);
+		Self::expend_cycles(cycles, 1);
+
+		data
+	}
+
 	/// Sets the LDA status after executing an LDA operation.
 	fn lda_set_status(&mut self) {
 		trace!("setting LDA status");
@@ -125,6 +149,20 @@ impl Cpu {
 		data
 	}
 
+	/// Writes a word to memory.
+	fn write_word(cycles: &mut u32, data: u16, address: u32, memory: &mut Memory) {
+		debug!("writing word to address {address:#08X}");
+
+		memory
+			.data
+			.get_mut((address as usize)..(address + 2) as usize)
+			.expect("write_word address indexed out of bounds")
+			.write_u16::<LittleEndian>(data)
+			.expect("failed to write data");
+
+		Self::expend_cycles(cycles, 2);
+	}
+
 	/// Expends the specified number of cycles. This method is for internal use to log expended
 	/// cycles.
 	fn expend_cycles(cycles: &mut u32, count: u32) {
@@ -134,7 +172,7 @@ impl Cpu {
 
 	/// Executes instructions on the [`Cpu`] from memory. Runs the specified
 	/// number of cycles.
-	pub fn execute(&mut self, cycles: u32, memory: &Memory) {
+	pub fn execute(&mut self, cycles: u32, memory: &mut Memory) {
 		// Shadow cycles with a mutable copy
 		let mut cycles = cycles;
 
@@ -152,6 +190,32 @@ impl Cpu {
 				);
 
 				match (operation.instruction, operation.addressing_mode) {
+					(Instruction::Jsr, AddressingMode::Absolute) => {
+						// Fetch the subroutine address from memory.
+						let subroutine_address: u16 = self.fetch_word(&mut cycles, memory);
+
+						// Write the program counter - 1 to the stack.
+						Self::write_word(
+							&mut cycles,
+							self.program_counter - 1,
+							self.stack_pointer.into(),
+							memory,
+						);
+
+						// Increment stack pointer.
+						self.stack_pointer += 1;
+
+						// Set program counter to subroutine address.
+						self.program_counter = subroutine_address;
+						Self::expend_cycles(&mut cycles, 1);
+					}
+					(
+						Instruction::Jsr,
+						AddressingMode::Immediate
+						| AddressingMode::ZeroPage
+						| AddressingMode::ZeroPageX,
+					) => unreachable!(),
+					(Instruction::Lda, AddressingMode::Absolute) => todo!(),
 					(Instruction::Lda, AddressingMode::Immediate) => {
 						let value = self.fetch_byte(&mut cycles, memory);