From 17c0cb8660a8f0c6dcc9e5409f30efb97b92718e Mon Sep 17 00:00:00 2001
From: AnonymusRaccoon <tr.tristan-roux@outlook.fr>
Date: Wed, 12 Feb 2020 16:40:06 +0100
Subject: [PATCH] Adding the reset

---
 sources/CPU/CPU.cpp                     | 26 ++++++++++++++++++++++---
 sources/CPU/CPU.hpp                     | 14 ++++++++++++-
 sources/CPU/Instructions/Interrupts.cpp | 17 ++++++++++++++++
 3 files changed, 53 insertions(+), 4 deletions(-)

diff --git a/sources/CPU/CPU.cpp b/sources/CPU/CPU.cpp
index 0770183..f1c3e4f 100644
--- a/sources/CPU/CPU.cpp
+++ b/sources/CPU/CPU.cpp
@@ -15,7 +15,7 @@ namespace ComSquare::CPU
 	CPU::CPU(std::shared_ptr<Memory::MemoryBus> bus, Cartridge::Header &cartridgeHeader)
 		: _bus(std::move(bus)), _cartridgeHeader(cartridgeHeader)
 	{
-		this->_registers.pc = this->_cartridgeHeader.emulationInterrupts.reset;
+		this->RESB();
 	}
 
 	//! @bref The CPU's internal registers starts at $4200	and finish at $421F.
@@ -220,11 +220,31 @@ namespace ComSquare::CPU
 		case Instructions::ADC_SRYi: return 7 + this->ADC(this->_getStackRelativeIndirectIndexedYAddr());
 
 		default:
-			return 0;
-			//throw InvalidOpcode("CPU", opcode);
+			throw InvalidOpcode("CPU", opcode);
 		}
 	}
 
+	void CPU::push(uint8_t data)
+	{
+		this->_bus->write(this->_registers.s--, data);
+	}
+
+	void CPU::push(uint16_t data)
+	{
+		this->_bus->write(this->_registers.s--, data);
+		this->_bus->write(this->_registers.s--, data << 8u);
+	}
+
+	uint8_t CPU::pop()
+	{
+		return this->_bus->read(this->_registers.s++);
+	}
+
+	uint16_t CPU::pop16()
+	{
+		return this->_bus->read(this->_registers.s++) + (this->_bus->read(this->_registers.s++) << 8u);
+	}
+
 	////////////////////////////////////////////////////////////////////
 	/// Addressing modes
 	////////////////////////////////////////////////////////////////////
diff --git a/sources/CPU/CPU.hpp b/sources/CPU/CPU.hpp
index b50d46e..c864ee8 100644
--- a/sources/CPU/CPU.hpp
+++ b/sources/CPU/CPU.hpp
@@ -202,7 +202,7 @@ namespace ComSquare::CPU
 		ADC_DPYil = 0x77,
 		ADC_ABSY = 0x79,
 		ADC_ABSX = 0x7D,
-		ADC_ABSXl = 0x7F
+		ADC_ABSXl = 0x7F,
 	};
 
 	//! @brief The main CPU
@@ -264,10 +264,22 @@ namespace ComSquare::CPU
 		uint24_t _getStackRelativeIndirectIndexedYAddr();
 
 
+		//! @brief Push 8 bits of data to the stack.
+		void push(uint8_t data);
+		//! @brief Push 16 bits of data to the stack.
+		void push(uint16_t data);
+		//! @brief Pop 8 bits of data from the stack.
+		uint8_t pop();
+		//! @brief Pop 16 bits of data from the stack.
+		uint16_t pop16();
+
+
 		//! @brief Execute a single instruction.
 		//! @return The number of CPU cycles that the instruction took.
 		unsigned executeInstruction();
 
+		//! @brief Reset interrupt - Called on boot and when the reset button is pressed.
+		unsigned RESB();
 		//! @brief Break instruction - Causes a software break. The PC is loaded from a vector table.
 		unsigned BRK();
 		//! @brief Add with carry - Adds operand to the Accumulator; adds an additional 1 if carry is set.
diff --git a/sources/CPU/Instructions/Interrupts.cpp b/sources/CPU/Instructions/Interrupts.cpp
index ee29629..8136db9 100644
--- a/sources/CPU/Instructions/Interrupts.cpp
+++ b/sources/CPU/Instructions/Interrupts.cpp
@@ -6,8 +6,25 @@
 
 namespace ComSquare::CPU
 {
+	unsigned CPU::RESB()
+	{
+		this->_registers.p.i = true;
+		this->_registers.p.d = false;
+		this->_isEmulationMode = true;
+		this->_registers.p.m = true;
+		this->_registers.p.x_b = true;
+		this->_registers.dbr = 0x00;
+		this->_registers.pbr = 0x00;
+		this->_registers.d = 0x0000;
+		this->_registers.sh = 0x01; // the low bit of the stack pointer is undefined on reset.
+		this->_registers.pc = this->_cartridgeHeader.emulationInterrupts.reset;
+		return 0;
+	}
+
 	unsigned CPU::BRK()
 	{
+		// TODO rework this. The PC should be pushed to the stack.
+		// Info here: http://softpixel.com/~cwright/sianse/docs/65816NFO.HTM at BRK Software Break
 		this->_registers.pc += 2;
 
 		this->_registers.p.i = true;