From e43a0a9ae129a8b552d7e1ecf77c92301bc8febb Mon Sep 17 00:00:00 2001
From: Ivan Izaguirre <ivanizag@gmail.com>
Date: Sat, 2 Feb 2019 22:53:26 +0100
Subject: [PATCH] Support for stack instructions PHA, PHP, PLA and PLP

---
 execute.go      | 30 +++++++++++++++++++++++++-----
 execute_test.go | 39 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 64 insertions(+), 5 deletions(-)

diff --git a/execute.go b/execute.go
index 470a8a8..362026f 100644
--- a/execute.go
+++ b/execute.go
@@ -247,6 +247,26 @@ func buildOpSub(addressMode int) opFunc {
 	}
 }
 
+const stackAddress uint16 = 0x0100
+
+func buildOpPush(reg int) opFunc {
+	return func(s *state, line []uint8, opcode opcode) {
+		value := s.registers.getRegister(reg)
+		adresss := stackAddress + uint16(s.registers.getSP())
+		s.memory[adresss] = value
+		s.registers.setSP(s.registers.getSP() - 1)
+	}
+}
+
+func buildOpPull(reg int) opFunc {
+	return func(s *state, line []uint8, opcode opcode) {
+		s.registers.setSP(s.registers.getSP() + 1)
+		adresss := stackAddress + uint16(s.registers.getSP())
+		value := s.memory[adresss]
+		s.registers.setRegister(reg, value)
+	}
+}
+
 /*
 TODO:
 http://www.6502.org/tutorials/decimal_mode.html
@@ -258,14 +278,14 @@ JSR
 RTI
 RTS
 
-PHA
-PHP
-PLA
-PLP
-
 */
 
 var opcodes = [256]opcode{
+	0x48: opcode{"PHA", 1, 3, buildOpPush(regA)},
+	0x08: opcode{"PHP", 1, 3, buildOpPush(regP)},
+	0x68: opcode{"PLA", 1, 4, buildOpPull(regA)},
+	0x28: opcode{"PLP", 1, 4, buildOpPull(regP)},
+
 	0x09: opcode{"ORA", 2, 2, buildOpLogic(modeImmediate, operationOr)},
 	0x05: opcode{"ORA", 2, 3, buildOpLogic(modeZeroPage, operationOr)},
 	0x15: opcode{"ORA", 2, 4, buildOpLogic(modeZeroPageX, operationOr)},
diff --git a/execute_test.go b/execute_test.go
index 9e58b47..fcce26d 100644
--- a/execute_test.go
+++ b/execute_test.go
@@ -408,3 +408,42 @@ func TestBranch(t *testing.T) {
 		t.Errorf("Error in BCC, %v", s.registers)
 	}
 }
+
+func TestStack(t *testing.T) {
+	var s state
+
+	s.registers.setSP(0xF0)
+	s.registers.setA(0xA0)
+	s.registers.setP(0x0A)
+	executeLine(&s, []uint8{0x48})
+	if s.registers.getSP() != 0xEF {
+		t.Errorf("Error in PHA stack pointer, %v", s.registers)
+	}
+	if s.memory[0x01F0] != 0xA0 {
+		t.Errorf("Error in PHA, %v", s.registers)
+	}
+
+	executeLine(&s, []uint8{0x08})
+	if s.registers.getSP() != 0xEE {
+		t.Errorf("Error in PHP stack pointer, %v", s.registers)
+	}
+	if s.memory[0x01EF] != 0x0A {
+		t.Errorf("Error in PHP, %v", s.registers)
+	}
+
+	executeLine(&s, []uint8{0x68})
+	if s.registers.getSP() != 0xEF {
+		t.Errorf("Error in PLA stack pointer, %v", s.registers)
+	}
+	if s.registers.getA() != 0x0A {
+		t.Errorf("Error in PLA, %v", s.registers)
+	}
+
+	executeLine(&s, []uint8{0x28})
+	if s.registers.getSP() != 0xF0 {
+		t.Errorf("Error in PLP stack pointer, %v", s.registers)
+	}
+	if s.registers.getP() != 0xA0 {
+		t.Errorf("Error in PLP, %v", s.registers)
+	}
+}