Implement memory device

The memory device is fully implemented, with 3MB of heap memory.

This commit is a bit messy, additional changes are:
- The program memory and program counter in each Bedrock struct have
  been moved to a dedicated struct to prevent a name collision with the
  memory device
- The run_bg and debug functions have been moved to core.c and debug.c
- The blank screen colour has been changed back to black
- No second program runs on the sub screen by default
- The number of Bedrock instances to run has been parameterized

3 files changed, 186 insertions, 3 deletions

diff --git a/arm9/source/devices/memory.c b/arm9/source/devices/memory.c
new file mode 100644
index 0000000..915aec2
--- /dev/null
+++ b/arm9/source/devices/memory.c
@@ -0,0 +1,143 @@
+#include "nds.h"
+#include "memory.h"
+
+static u8 heap[HEAP_SIZE][256];  // the page heap.
+static u8 unallocated[256];      // page to use for all unallocated reads and writes.
+static u8 heap_index[HEAP_SIZE];
+
+u8 mem_read1(MemoryDevice *mem) {
+    u8 output = mem->point1[mem->byte1++];
+    if (mem->byte1 == 0) {
+        mem->page1++;
+        mem_get_page1(mem);
+    }
+    return output;
+}
+
+u8 mem_read2(MemoryDevice *mem) {
+    u8 output = mem->point2[mem->byte2++];
+    if (mem->byte2 == 0) {
+        mem->page2++;
+        mem_get_page2(mem);
+    }
+    return output;
+}
+
+void mem_write1(MemoryDevice *mem, u8 value) {
+    mem->point1[mem->byte1++] = value;
+    if (mem->byte1 == 0) {
+        mem->page1++;
+        mem_get_page1(mem);
+    }
+}
+
+void mem_write2(MemoryDevice *mem, u8 value) {
+    mem->point2[mem->byte2++] = value;
+    if (mem->byte2 == 0) {
+        mem->page2++;
+        mem_get_page2(mem);
+    }
+}
+
+
+void mem_load_cache1(MemoryDevice *mem) {
+    // Set all cached pointers to unallocated.
+    for (int i=0; i<256; i++) {
+        mem->cache1[i] = (u8*) &unallocated;
+    }
+    // Iterate over all heap pages, gather our ones.
+    int count = 0;
+    int cached = 0;
+    for (int i=0; i<HEAP_SIZE; i++) {
+        if (heap_index[i] == mem->id) {
+            if (count >= mem->offset1) {
+                mem->cache1[cached] = (u8*) &heap[i];
+                cached++;
+            }
+            count++;
+            if (cached == 256) break;
+        }
+    }
+}
+
+void mem_load_cache2(MemoryDevice *mem) {
+    // Set all cached pointers to unallocated.
+    for (int i=0; i<256; i++) {
+        mem->cache2[i] = (u8*) &unallocated;
+    }
+    // Iterate over all heap pages, gather our ones.
+    int count = 0;
+    int cached = 0;
+    for (int i=0; i<HEAP_SIZE; i++) {
+        if (heap_index[i] == mem->id) {
+            if (count >= mem->offset2) {
+                mem->cache2[cached] = (u8*) &heap[i];
+                cached++;
+            }
+            count++;
+            if (cached == 256) break;
+        }
+    }
+}
+
+// Fetch the page1 pointer from cache1.
+void mem_get_page1(MemoryDevice *mem) {
+    mem->point1 = mem->cache1[mem->page1];
+}
+
+// Fetch the page2 pointer from cache2.
+void mem_get_page2(MemoryDevice *mem) {
+    mem->point2 = mem->cache2[mem->page2];
+}
+
+void mem_allocate(MemoryDevice *mem) {
+    int count = 0;
+
+    // ALLOCATING
+    if (mem->count_write > mem->count) {
+        int to_allocate = mem->count_write - mem->count;
+        for (int i=0; i<HEAP_SIZE; i++) {
+            if (heap_index[i] == 0) {
+                heap_index[i] = mem->id;
+                count++;
+                if (count == to_allocate) {
+                    break;
+                }
+            }
+        }
+    // DEALLOCATING
+    } else if (mem->count_write < mem->count) {
+        for (int i=0; i<HEAP_SIZE; i++) {
+            if (heap_index[i] == mem->id) {
+                count++;
+                if (count > mem->count_write) {
+                    heap_index[i] = 0;
+                    memset(heap[i], 0, 256);
+                }
+            }
+        }
+    }
+
+    // Count the number of pages allocated to us.
+    mem->count = 0;
+    for (int i=0; i<HEAP_SIZE; i++) {
+        if (heap_index[i]==mem->id) mem->count++;
+    }
+    // Reload the pointer caches for each head.
+    mem_load_cache1(mem);
+    mem_load_cache2(mem);
+}
+
+
+void mem_do_copy(MemoryDevice *mem) {
+    int src = mem->offset2;
+    int dest = mem->offset1;
+    if (src == dest) return;
+    for (int i=0; i<mem->copy_write; i++) {
+        if (src>=mem->count || dest>=mem->count) {
+            return;
+        }
+        memcpy(&heap[dest++], &heap[src++], 256);
+    }
+}
+
diff --git a/arm9/source/devices/memory.h b/arm9/source/devices/memory.h
new file mode 100644
index 0000000..0abff16
--- /dev/null
+++ b/arm9/source/devices/memory.h
@@ -0,0 +1,42 @@
+#ifndef MEMORY_H_
+    #define MEMORY_H_
+
+    #define HEAP_SIZE (4096*3)
+
+    typedef struct {
+        u8 id;           // Unique non-zero identifier for this memory device.
+
+        u16 offset1;     // Bedrock offset value for head 1
+        u8 page1;        // Bedrock address value for head 1
+        u8 byte1;        // Bedrock address value for head 1
+        u16 offset2;     // Bedrock offset value for head 2
+        u8 page2;        // Bedrock address value for head 2
+        u8 byte2;        // Bedrock address value for head 2
+
+        u16 count_write;  // write cache for page count
+        u16 count;        // number of pages allocated for this bedrock
+        u16 copy_write;   // write cache for page copy length
+
+        u8 *point1;     // pointer to current page for head 1
+        u8 *point2;     // pointer to current page for head 2
+
+        u8* cache1[256];
+        u8* cache2[256];
+    } MemoryDevice ;
+
+    u8 mem_read1(MemoryDevice *mem);
+    u8 mem_read2(MemoryDevice *mem);
+    void mem_write1(MemoryDevice *mem, u8 value);
+    void mem_write2(MemoryDevice *mem, u8 value);
+
+    void mem_load_cache1(MemoryDevice *mem);
+    void mem_load_cache2(MemoryDevice *mem);
+    void mem_get_page1(MemoryDevice *mem);
+    void mem_get_page2(MemoryDevice *mem);
+
+
+    void mem_allocate(MemoryDevice *mem);
+    void mem_do_copy(MemoryDevice *mem);
+
+
+#endif
diff --git a/arm9/source/devices/screen.c b/arm9/source/devices/screen.c
index 492bb05..0d2fc36 100644
--- a/arm9/source/devices/screen.c
+++ b/arm9/source/devices/screen.c
@@ -52,8 +52,6 @@ void set_palette_low(ScreenDevice *scr, u8 low) {
     u8 r = scr->palette_write >>  7 & 0x1e;
     u8 g = scr->palette_write >>  3 & 0x1e;
     u8 b = scr->palette_write <<  1 & 0x1e;
-    // TODO: With 4-bit we multiply by 17, find equivalent
-    //       here to get best value of least-significant bit.
     scr->palette[i] = RGB15(r,g,b);
     scr->nds->pal[i] = RGB15(r,g,b);
 }
@@ -132,7 +130,7 @@ void flip_buffer(Screen *nds) {
 }
 
 void black_screen(Screen *nds) {
-    nds->pal[0] = RGB15(8,0,0);
+    nds->pal[0] = RGB15(0,0,0);
     dmaFillWords(0, nds->bgv, TILES_MEM);
     dmaFillWords(0, nds->fgv, TILES_MEM);
 }