From 77fa2964f129a2e653787e8fcbf41296d39dcdb0 Mon Sep 17 00:00:00 2001
From: Takashi Toyoshima <toyoshim@gmail.com>
Date: Mon, 8 Dec 2014 02:43:05 +0900
Subject: [PATCH] Bug fix: stack is hard :(   SP should wrap correctly on push
 and pop

---
 6502.S   | 47 +++++++++++++++++++++++++++++++++++++++--------
 Makefile |  2 +-
 ftest.c  | 13 ++++++++++---
 test.c   |  2 +-
 4 files changed, 51 insertions(+), 13 deletions(-)

diff --git a/6502.S b/6502.S
index 08076a3..31edb85 100644
--- a/6502.S
+++ b/6502.S
@@ -91,6 +91,11 @@
   _stb
   mov  r0, SP
   subs r0, r0, #1
+  cmp  r0, #0xff
+  bne  1f
+  lsls r0, r0, #1
+  adds r0, r0, #1
+1:
   mov  SP, r0
 .endm
 
@@ -101,30 +106,55 @@
   _stb
   mov  r0, SP
   subs r0, r0, #1
+  cmp  r0, #0xff
+  bne  1f
+  lsls r0, r0, #1
+  adds r0, r0, #1
+1:
+  mov  SP, r0
   mov  r1, \reg
   _stb
   mov  r0, SP
-  subs r0, r0, #2
+  subs r0, r0, #1
+  cmp  r0, #0xff
+  bne  1f
+  lsls r0, r0, #1
+  adds r0, r0, #1
+1:
   mov  SP, r0
 .endm
 
 .macro _popb
   mov  r0, SP
   adds r0, r0, #1
+  lsrs r1, r0, #10
+  bcc  1f
+  lsrs r0, r0, #1
+1:
   mov  SP, r0
   _ldb
 .endm
 
 .macro _popw
-  mov  T0, SP
-  adds r0, T0, #1
+  mov  r0, SP
+  adds r0, r0, #1
+  lsrs r1, r0, #10
+  bcc  1f
+  lsrs r0, r0, #1
+1:
+  mov  SP, r0
   _ldb
-  mov  T1, r0
-  adds r0, T0, #2
+  mov  T0, r0
+  mov  r0, SP
+  adds r0, r0, #1
+  lsrs r1, r0, #10
+  bcc  1f
+  lsrs r0, r0, #1
+1:
   mov  SP, r0
   _ldb
   lsls r0, r0, #8
-  adds r0, r0, T1
+  adds r0, r0, T0
 .endm
 
 .macro _decode
@@ -1271,10 +1301,11 @@ op99:  // STA - Absolute, Y
   _decode
 
 op9a:  // TXS
-  _t   RX, SP
   movs r0, #1
   lsls r0, r0, #8
-  add  SP, SP, r0
+  add  r0, RX, r0
+  mov  SP, r0
+  adds PC, PC, #1
   _decode
 
 op9d:  // STA - Absolute, X
diff --git a/Makefile b/Makefile
index f277d99..4bbc445 100644
--- a/Makefile
+++ b/Makefile
@@ -54,7 +54,7 @@ run: $(APP).bin
 	$(LPC21ISP) -control -term -bin $(APP).bin $(SERIAL) $(SPEED) $(CLOCK)
 
 clean:
-	rm -rf $(APP).bin $(APP) $(OBJS) test
+	rm -rf $(APP).bin $(APP) $(OBJS) test ftest
 
 # Use this build target to install required packages if you are on Ubuntu14.04.
 install-deps:
diff --git a/ftest.c b/ftest.c
index 9283aa0..a26af50 100644
--- a/ftest.c
+++ b/ftest.c
@@ -1,3 +1,4 @@
+#include <assert.h>
 #include <stdio.h>
 #include <stdint.h>
 #include <stdlib.h>
@@ -14,15 +15,21 @@ void prn(int c) {
   __asm__("movs r12, %0":: "r"(fp));
 }
 
-void cpu6502_dump(
-    uint16_t pc, uint8_t a, uint8_t x, uint8_t y, uint8_t sp, uint8_t sr) {
+void cpu6502_dump(uint32_t pc, uint32_t a, uint32_t x, uint32_t y,
+                  uint32_t sp, uint32_t sr) {
   uint8_t fp;
   __asm__("movs %0, r12": "=r"(fp));
   fprintf(stderr, "*** dump *** PC=$%04x A=$%02x X=$%02x Y=$%02x SP=$%02x "
           "NV-B_DIZC=%d%d-%d_%d%d%d%d\n",
-          pc, a, x, y, sp, (sr >> 7) & 1, (sr >> 6) & 1, (sr >> 4) & 1,
+          pc, a, x, y, sp & 0xff, (sr >> 7) & 1, (sr >> 6) & 1, (sr >> 4) & 1,
           (sr >> 3) & 1, (sr >> 2) & 1, (sr >> 1) & 1, sr & 1);
   fflush(stderr);
+  assert(pc < 0x10000);
+  assert(a < 0x100);
+  assert(x < 0x100);
+  assert(y < 0x100);
+  assert(sr < 0x100);
+  assert(0x100 <= sp && sp < 0x200);
   __asm__("movs r12, %0":: "r"(fp));
 }
 
diff --git a/test.c b/test.c
index 3adc8dd..0785d2d 100644
--- a/test.c
+++ b/test.c
@@ -28,7 +28,7 @@ void cpu6502_dump(uint32_t pc, uint32_t a, uint32_t x, uint32_t y,
   assert(0x100 <= sp && sp < 0x200);
   fprintf(stderr, "*** dump *** PC=$%04x A=$%02x X=$%02x Y=$%02x SP=$%02x "
           "NV-B_DIZC=%d%d-%d_%d%d%d%d\n",
-          pc, a, x, y, sp, (sr >> 7) & 1, (sr >> 6) & 1, (sr >> 4) & 1,
+          pc, a, x, y, sp & 0xff, (sr >> 7) & 1, (sr >> 6) & 1, (sr >> 4) & 1,
           (sr >> 3) & 1, (sr >> 2) & 1, (sr >> 1) & 1, sr & 1);
   fflush(stderr);
   __asm__("movs r12, %0":: "r"(fp));