From 8800bf2b9f4948d8d043f48406b1c3d71fc3fe7d Mon Sep 17 00:00:00 2001
From: Stefan Arentz <stefan@arentz.ca>
Date: Sun, 15 Jan 2017 20:48:16 -0500
Subject: [PATCH] Fixes #139 Optimize zero page and stack access (#140)

---
 src/cpu.c | 46 +++++++++++++++++++++++++++++++++++++++-------
 src/cpu.h |  5 +++++
 src/mem.c | 30 ++++++++++++++----------------
 3 files changed, 58 insertions(+), 23 deletions(-)

diff --git a/src/cpu.c b/src/cpu.c
index 9d4d3bb..ac18908 100644
--- a/src/cpu.c
+++ b/src/cpu.c
@@ -40,22 +40,20 @@
 // Stack management.
 
 void _cpu_push_byte(struct cpu_t *cpu, uint8_t b) {
-   mem_set_byte(cpu, 0x0100 + cpu->state.sp, b);
-  cpu->state.sp -= 1;
+   cpu->page1[cpu->state.sp--] = b;
 }
 
 void _cpu_push_word(struct cpu_t *cpu, uint16_t w) {
-  _cpu_push_byte(cpu, (uint8_t) (w >> 8));
-  _cpu_push_byte(cpu, (uint8_t) w);
+   cpu->page1[cpu->state.sp--] = w >> 8;
+   cpu->page1[cpu->state.sp--] = w;
 }
 
 uint8_t _cpu_pull_byte(struct cpu_t *cpu) {
-  cpu->state.sp += 1;
-  return mem_get_byte(cpu, 0x0100 + cpu->state.sp);
+   return cpu->page1[++cpu->state.sp];
 }
 
 uint16_t _cpu_pull_word(struct cpu_t *cpu) {
-  return (uint16_t) _cpu_pull_byte(cpu) | ((uint16_t) _cpu_pull_byte(cpu) << 8);
+   return (uint16_t) cpu->page1[++cpu->state.sp] | ((uint16_t) cpu->page1[++cpu->state.sp] << 8);
 }
 
 uint8_t _cpu_stack_free(struct cpu_t *cpu) {
@@ -212,6 +210,17 @@ void cpu_destroy(struct cpu_t *cpu) {
    }
 }
 
+static struct mem_t *cpu_mem_for_page(struct cpu_t *cpu, uint8_t page) {
+   struct mem_t *mem = cpu->mem;
+   while (mem != NULL) {
+      if (mem->enabled && ((page * 0x100) >= mem->start) && ((page * 0x0100 + 0xff) <= mem->end)) {
+         return mem;
+      }
+      mem = mem->next;
+   }
+   return NULL;
+}
+
 struct mem_t *cpu_add_mem(struct cpu_t *cpu, struct mem_t *mem) {
   if (cpu->mem == NULL) {
     cpu->mem = mem;
@@ -345,6 +354,27 @@ struct mem_t *cpu_add_iom(struct cpu_t *cpu, uint16_t start, uint16_t end, void
   return cpu_add_mem(cpu, mem);
 }
 
+// Call this when the memory layout changes for page 0 and 1. For
+// example when those pages are bankswitched. I don't think anything
+// does that right now. The Apple Language Card works in different
+// memory regions.
+
+void cpu_optimize_memory(struct cpu_t *cpu) {
+   struct mem_t *page0 = cpu_mem_for_page(cpu, 0);
+   if (page0 == NULL || (page0->flags != (MEM_FLAGS_READ | MEM_FLAGS_WRITE))) {
+      printf("[CPU] Cannot find rw memory region that handles Page 0\n");
+      exit(1);
+   }
+   cpu->page0 = page0->obj + (0x0000 - page0->start);
+
+   struct mem_t *page1 = cpu_mem_for_page(cpu, 1);
+   if (page1 == NULL || (page1->flags != (MEM_FLAGS_READ | MEM_FLAGS_WRITE))) {
+      printf("[CPU] Cannot find rw memory region that handles Page 1\n");
+      exit(1);
+   }
+   cpu->page1 = page1->obj + (0x0100 - page1->start);
+}
+
 void cpu_strict(struct cpu_t *cpu, bool strict) {
    cpu->strict = strict;
 }
@@ -378,6 +408,8 @@ void cpu_reset(struct cpu_t *cpu) {
    cpu->state.z = 0;
    cpu->state.c = 0;
    cpu->state.sp = 0xff;
+
+   cpu_optimize_memory(cpu);
 }
 
 int cpu_irq(struct cpu_t *cpu) {
diff --git a/src/cpu.h b/src/cpu.h
index 4189451..0cb230e 100644
--- a/src/cpu.h
+++ b/src/cpu.h
@@ -54,6 +54,9 @@ struct cpu_t {
    struct mem_t *mem;
    struct cpu_instruction_t *instructions;
    uint64_t counter;
+
+   uint8_t *page0;
+   uint8_t *page1;
 };
 
 typedef void (*cpu_instruction_handler_t)(struct cpu_t *cpu);
@@ -102,6 +105,8 @@ struct mem_t *cpu_add_rom_data(struct cpu_t *cpu, uint16_t start, uint16_t end,
 struct mem_t *cpu_add_rom_file(struct cpu_t *cpu, uint16_t start, char *path);
 struct mem_t *cpu_add_iom(struct cpu_t *cpu, uint16_t start, uint16_t end, void *obj, mem_read_handler_t read_handler, mem_write_handler_t write_handler);
 
+void cpu_optimize_memory(struct cpu_t *cpu);
+
 void cpu_strict(struct cpu_t *cpu, bool strict);
 int cpu_trace(struct cpu_t *cpu, char *path);
 
diff --git a/src/mem.c b/src/mem.c
index d4d05cd..15b9be0 100644
--- a/src/mem.c
+++ b/src/mem.c
@@ -74,54 +74,53 @@ uint8_t mem_get_byte_abs(struct cpu_t *cpu, uint16_t addr) {
 }
 
 uint8_t mem_get_byte_absx(struct cpu_t *cpu, uint16_t addr) {
-  return mem_get_byte(cpu, addr + cpu->state.x); /* TODO: Carry? */
+   return mem_get_byte(cpu, addr + cpu->state.x);
 }
 
 uint8_t mem_get_byte_absy(struct cpu_t *cpu, uint16_t addr) {
-  return mem_get_byte(cpu, addr + cpu->state.y); /* TODO: Carry? */
+  return mem_get_byte(cpu, addr + cpu->state.y);
 }
 
 uint8_t mem_get_byte_zpg(struct cpu_t *cpu, uint8_t addr) {
-  return mem_get_byte(cpu, addr);
+   return cpu->page0[addr];
 }
 
 uint8_t mem_get_byte_zpgx(struct cpu_t *cpu, uint8_t addr) {
-  return mem_get_byte(cpu, ((uint16_t) addr + cpu->state.x) & 0x00ff);
+   return cpu->page0[((uint16_t) addr + cpu->state.x) & 0x00ff];
 }
 
 uint8_t mem_get_byte_zpgy(struct cpu_t *cpu, uint8_t addr) {
-  return mem_get_byte(cpu, ((uint16_t) addr + cpu->state.y) & 0x00ff);
+   return cpu->page0[((uint16_t) addr + cpu->state.y) & 0x00ff];
 }
 
 uint8_t mem_get_byte_indx(struct cpu_t *cpu, uint8_t addr) {
-  return mem_get_byte(cpu, mem_get_word(cpu, (uint8_t)(addr + cpu->state.x)));
+   return mem_get_byte(cpu, (((uint16_t) cpu->page0[((uint16_t)addr+1+cpu->state.x)&0x00ff] << 8) | (uint16_t) cpu->page0[((uint16_t) addr+cpu->state.x) & 0x00ff]));
 }
 
 uint8_t mem_get_byte_indy(struct cpu_t *cpu, uint8_t addr) {
-  return mem_get_byte(cpu, mem_get_word(cpu, addr) + cpu->state.y);
+   return mem_get_byte(cpu, (((uint16_t) cpu->page0[addr+1] << 8) | (uint16_t) cpu->page0[addr]) + cpu->state.y);
 }
 
 uint8_t mem_get_byte_ind(struct cpu_t *cpu, uint8_t addr) {
-  return mem_get_byte(cpu, mem_get_word(cpu, addr));
+   return mem_get_byte(cpu, ((uint16_t) cpu->page0[addr+1] << 8) | (uint16_t) cpu->page0[addr]);
 }
 
 uint16_t mem_get_word(struct cpu_t *cpu, uint16_t addr) {
-  // TODO Did I do this right?
   return ((uint16_t) mem_get_byte(cpu, addr+1) << 8) | (uint16_t) mem_get_byte(cpu, addr);
 }
 
 // Setters
 
 void mem_set_byte_zpg(struct cpu_t *cpu, uint8_t addr, uint8_t v) {
-  mem_set_byte(cpu, addr, v);
+   cpu->page0[addr] = v;
 }
 
 void mem_set_byte_zpgx(struct cpu_t *cpu, uint8_t addr, uint8_t v) {
-  mem_set_byte(cpu, ((uint16_t) addr + cpu->state.x) & 0x00ff, v);
+   cpu->page0[((uint16_t) addr + cpu->state.x) & 0x00ff] = v;
 }
 
 void mem_set_byte_zpgy(struct cpu_t *cpu, uint8_t addr, uint8_t v) {
-  mem_set_byte(cpu, ((uint16_t) addr + cpu->state.y) & 0x00ff, v);
+   cpu->page0[((uint16_t) addr + cpu->state.y) & 0x00ff] = v;
 }
 
 void mem_set_byte_abs(struct cpu_t *cpu, uint16_t addr, uint8_t v) {
@@ -137,16 +136,15 @@ void mem_set_byte_absy(struct cpu_t *cpu, uint16_t addr, uint8_t v) {
 }
 
 void mem_set_byte_indx(struct cpu_t *cpu, uint8_t addr, uint8_t v) {
-  //uint8_t a = ;
-  mem_set_byte(cpu, mem_get_word(cpu, (uint8_t)(addr + cpu->state.x)), v);
+   mem_set_byte(cpu, (((uint16_t) cpu->page0[((uint16_t)addr+1+cpu->state.x)&0x00ff] << 8) | (uint16_t) cpu->page0[((uint16_t) addr+cpu->state.x) & 0x00ff]), v);
 }
 
 void mem_set_byte_indy(struct cpu_t *cpu, uint8_t addr, uint8_t v) {
-  mem_set_byte(cpu, mem_get_word(cpu, addr)+cpu->state.y, v);
+   mem_set_byte(cpu, (((uint16_t) cpu->page0[addr+1] << 8) | (uint16_t) cpu->page0[addr]) + cpu->state.y, v);
 }
 
 void mem_set_byte_ind(struct cpu_t *cpu, uint8_t addr, uint8_t v) {
-  mem_set_byte(cpu, mem_get_word(cpu, addr), v);
+   mem_set_byte(cpu, (((uint16_t) cpu->page0[addr+1] << 8) | (uint16_t) cpu->page0[addr]), v);
 }
 
 void mem_set_word(struct cpu_t *cpu, uint16_t addr, uint16_t v) {