From a863b36f821da5e3a843ff67bc2d7fabc1a17d6d Mon Sep 17 00:00:00 2001
From: PalindromicBreadLoaf <herbthehaircut@proton.me>
Date: Tue, 22 Jul 2025 11:41:06 -0400
Subject: [PATCH] Implement Some Subroutine instructions

---
 src/cpu.cpp | 101 +++++++++++++++++++++++++++++++++++++++++++++++++++-
 src/cpu.h   |  12 +++++++
 2 files changed, 112 insertions(+), 1 deletion(-)

diff --git a/src/cpu.cpp b/src/cpu.cpp
index 1fbfbd5..41fc769 100644
--- a/src/cpu.cpp
+++ b/src/cpu.cpp
@@ -100,8 +100,9 @@ void CPU::UpdateCompareFlags16(const uint16_t reg_value, const uint16_t compare_
     }
 }
 
+// General Branching Code
 void CPU::DoBranch(const bool condition) {
-    int8_t displacement = static_cast<int8_t>(ReadByte(PC));
+    const int8_t displacement = static_cast<int8_t>(ReadByte(PC));
     PC++;
 
     if (condition) {
@@ -116,6 +117,32 @@ void CPU::DoBranch(const bool condition) {
     }
 }
 
+// Stack helper methods:
+void CPU::PushByte(const uint8_t value) {
+    WriteByte(SP, value);
+    SP--;
+    cycles++;
+}
+
+void CPU::PushWord(const uint16_t value) {
+    // Push high then low
+    PushByte(value >> 8);    // High byte
+    PushByte(value & 0xFF);  // Low byte
+}
+
+uint8_t CPU::PopByte() {
+    SP++;
+    cycles++;
+    return ReadByte(SP);
+}
+
+uint16_t CPU::PopWord() {
+    // Pop low then high
+    const uint8_t low = PopByte();
+    const uint8_t high = PopByte();
+    return (high << 8) | low;
+}
+
 void CPU::ExecuteInstruction() {
     // TODO: Actual Opcode decoding
     switch (const uint8_t opcode = bus->Read(PC++)) {
@@ -163,6 +190,11 @@ void CPU::ExecuteInstruction() {
         case 0x5C: JMP_AbsoluteLong(); break;       // JMP $nnnnnn
         case 0x7C: JMP_AbsoluteIndirectX(); break;  // JMP ($nnnn,X)
 
+        // Subroutine instructions
+        case 0x20: JSR_Absolute(); break;           // JSR $nnnn
+        case 0x22: JSR_AbsoluteLong(); break;       // JSR $nnnnnn
+        case 0xFC: JSR_AbsoluteIndirectX(); break;  // JSR ($nnnn,X)
+
         // Single Register Decrement
         case 0xCA: DEX(); break;                   // DEX - Decrement X Register
         case 0x88: DEY(); break;                   // DEY - Decrement Y Register
@@ -208,6 +240,10 @@ void CPU::ExecuteInstruction() {
         case 0xA4: LDY_DirectPage(); break;         // LDY $nn
         case 0xB4: LDY_DirectPageX(); break;        // LDY $nn,X
 
+        // More Subroutines
+        case 0x60: RTS(); break;                    // RTS
+        case 0x6B: RTL(); break;                    // RTL
+
         //SDA - Store Accumulator
         case 0x8D: STA_Absolute(); break;               // STA $nnnn
         case 0x9D: STA_AbsoluteX(); break;              // STA $nnnn,X
@@ -1515,4 +1551,67 @@ void CPU::BMI_Relative() {
 
 void CPU::BPL_Relative() {
     DoBranch(!(P & FLAG_N));
+}
+
+void CPU::JSR_Absolute() {
+    const uint16_t target_addr = ReadWord(PC);
+    PC += 2;
+
+    const uint16_t return_addr = (PC - 1) & 0xFFFF;
+    PushWord(return_addr);
+
+    PC = (static_cast<uint32_t>(PB) << 16) | target_addr;
+
+    cycles += 6;
+}
+
+void CPU::JSR_AbsoluteLong() {
+    const uint16_t addr_low = ReadWord(PC);
+    PC += 2;
+    const uint8_t addr_high = ReadByte(PC);
+    PC++;
+
+    PushByte(PB);
+
+    const uint16_t return_addr = (PC - 1) & 0xFFFF;
+    PushWord(return_addr);
+
+    PB = addr_high;
+    PC = (static_cast<uint32_t>(addr_high) << 16) | addr_low;
+
+    cycles += 8;
+}
+
+void CPU::JSR_AbsoluteIndirectX() {
+    const uint16_t base_addr = ReadWord(PC);
+    PC += 2;
+
+    const uint32_t indirect_addr = (static_cast<uint32_t>(PB) << 16) | ((base_addr + X) & 0xFFFF);
+
+    const uint16_t target_addr = ReadWord(indirect_addr);
+
+    const uint16_t return_addr = (PC - 1) & 0xFFFF;
+    PushWord(return_addr);
+
+    PC = (static_cast<uint32_t>(PB) << 16) | target_addr;
+
+    cycles += 8;
+}
+
+void CPU::RTS() {
+    const uint16_t return_addr = PopWord();
+
+    PC = (static_cast<uint32_t>(PB) << 16) | ((return_addr + 1) & 0xFFFF);
+
+    cycles += 6;
+}
+
+void CPU::RTL() {
+    const uint16_t return_addr = PopWord();
+
+    PB = PopByte();
+
+    PC = (static_cast<uint32_t>(PB) << 16) | ((return_addr + 1) & 0xFFFF);
+
+    cycles += 6;
 }
\ No newline at end of file
diff --git a/src/cpu.h b/src/cpu.h
index 52c3563..54444b3 100644
--- a/src/cpu.h
+++ b/src/cpu.h
@@ -63,6 +63,12 @@ public:
     // Branching helper method
     void DoBranch(bool condition);
 
+    // Helper methods for stack operations
+    void PushByte(uint8_t value);
+    void PushWord(uint16_t value);
+    uint8_t PopByte();
+    uint16_t PopWord();
+
     // Instruction implementations
     // TODO: Implement remaining instructions
     void CMP_Immediate();
@@ -158,6 +164,12 @@ public:
     void BCS_Relative();
     void BMI_Relative();
     void BPL_Relative();
+
+    void JSR_Absolute();
+    void JSR_AbsoluteLong();
+    void JSR_AbsoluteIndirectX();
+    void RTS();
+    void RTL();
 };
 
 #endif //CPU_H