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

8 files changed, 259 insertions, 28 deletions

diff --git a/arm9/source/core.c b/arm9/source/core.c
index cb112e0..66bd204 100644
--- a/arm9/source/core.c
+++ b/arm9/source/core.c
@@ -1,6 +1,7 @@
 #include <nds.h>
 #include "bang.h"
 #include "core.h"
+#include "debug.h"
 
 
 void reset_br(Bedrock *br) {
@@ -10,12 +11,24 @@ void reset_br(Bedrock *br) {
 // Load a program into an instance.
 void start_br(Bedrock *br, u8 program[], int size) {
     reset_br(br);
-    memcpy(&br->mem, program, size);
+    memcpy(&br->prg.mem, program, size);
     br->alive = TRUE;
     br->awake = TRUE;
     // br->scr.nds = &scr_main;
 }
 
+void run_br(Bedrock *br) {
+    if (br->awake) {
+        switch (evaluate(br, 1000)) {
+        case SIG_HALT:   br->alive = FALSE; black_screen(br->scr.nds); br = NULL; return;
+        case SIG_SLEEP:  br->awake = FALSE;            return;
+        case SIG_DB1:     debug_stacks(br);            return;
+        case SIG_DB4:     debug_assert(br);            return;
+        default:                                       return;
+        }
+    }
+}
+
 void rouse_br(Bedrock *br) {
     if (TEST(br->sys.sleep, 0x8000)) {  // SYSTEM
         br->awake = TRUE;
@@ -53,6 +66,21 @@ u8 dev_read(Bedrock *br, u8 port) {
     // TODO: available devices
     case 0x0A: return br->sys.wake;
 
+// MEMORY DEVICE
+    case 0x10: return mem_read1(&br->mem);
+    case 0x11: return mem_read1(&br->mem);
+    case 0x12: return HIGH(br->mem.offset1);
+    case 0x13: return  LOW(br->mem.offset1);
+    case 0x14: return      br->mem.page1;
+    case 0x15: return      br->mem.byte1;
+    case 0x16: return HIGH(br->mem.count);
+    case 0x17: return  LOW(br->mem.count);
+    case 0x18: return mem_read2(&br->mem);
+    case 0x19: return mem_read2(&br->mem);
+    case 0x1A: return HIGH(br->mem.offset2);
+    case 0x1B: return  LOW(br->mem.offset2);
+    case 0x1C: return      br->mem.page2;
+    case 0x1D: return      br->mem.byte2;
 // MATH DEVICE
     case 0x20: return HIGH(br->math.op1);
     case 0x21: return  LOW(br->math.op1);
@@ -117,6 +145,23 @@ Signal dev_write(Bedrock *br, u8 port, u8 v) {
 // SYSTEM DEVICE
     case 0x08: SET_HIGH(br->sys.sleep,v);          return 0;
     case 0x09:  SET_LOW(br->sys.sleep,v);  return SIG_SLEEP;
+// MEMORY DEVICE
+    case 0x10: mem_write1(&br->mem,v);                                  return 0;
+    case 0x11: mem_write1(&br->mem,v);                                  return 0;
+    case 0x12: SET_HIGH(br->mem.offset1,v); mem_load_cache1(&br->mem);  return 0;
+    case 0x13:  SET_LOW(br->mem.offset1,v); mem_load_cache1(&br->mem);  return 0;
+    case 0x14: br->mem.page1=v; mem_get_page1(&br->mem);                return 0;
+    case 0x15: br->mem.byte1=v;                                         return 0;
+    case 0x16: SET_HIGH(br->mem.count_write,v);                         return 0;
+    case 0x17:  SET_LOW(br->mem.count_write,v); mem_allocate(&br->mem); return 0;
+    case 0x18: mem_write2(&br->mem,v);                                  return 0;
+    case 0x19: mem_write2(&br->mem,v);                                  return 0;
+    case 0x1A: SET_HIGH(br->mem.offset2,v); mem_load_cache2(&br->mem);  return 0;
+    case 0x1B:  SET_LOW(br->mem.offset2,v); mem_load_cache2(&br->mem);  return 0;
+    case 0x1C: br->mem.page2=v; mem_get_page2(&br->mem);                return 0;
+    case 0x1D: br->mem.byte2=v;                                         return 0;
+    case 0x1E: SET_HIGH(br->mem.copy_write,v);                          return 0;
+    case 0x1F:  SET_LOW(br->mem.copy_write,v); mem_do_copy(&br->mem);   return 0;
 // MATH DEVICE
     case 0x20: set_op1_high(&br->math,v);          return 0;
     case 0x21: set_op1_low( &br->math,v);          return 0;
diff --git a/arm9/source/core.h b/arm9/source/core.h
index e5494ca..916806c 100644
--- a/arm9/source/core.h
+++ b/arm9/source/core.h
@@ -6,11 +6,12 @@
     #include "devices/math.h"
     #include "devices/screen.h"
     #include "devices/system.h"
+    #include "devices/memory.h"
 
     #define WST           br->wst
     #define RST           br->rst
-    #define MEM           br->mem
-    #define PC            br->pc
+    #define MEM           br->prg.mem
+    #define PC            br->prg.p
 
     #define WSTV(i)       WST.mem[WST.p+(i)]
     #define RSTV(i)       RST.mem[RST.p+(i)]
@@ -58,17 +59,23 @@
     } Signal;
 
     typedef struct {
+        u8 mem [65536];
+        u16 p;
+    } ProgramMemory;
+
+    typedef struct {
         u8 mem[256];            // stack memory
         u8 p;                   // stack pointer
-    } Stack;
+    } StackMemory;
 
     typedef struct {
         bool alive;             // true when program is loaded
         bool awake;             // true when program is not asleep
-        u8 mem[65536];          // program memory
-        u16 pc;                 // program counter
-        Stack wst, rst;         // working and return stacks
+        ProgramMemory prg;     // program memory
+        StackMemory wst, rst;   // working and return stacks
+
         SystemDevice sys;
+        MemoryDevice mem;
         MathDevice math;
         ClockDevice clk;
         InputDevice inp;
@@ -77,6 +84,7 @@
 
     void reset_br(Bedrock *br);
     void start_br(Bedrock *br, u8 program[], int size);
+    void run_br(Bedrock *br);
     void rouse_br(Bedrock *br);
     u8 dev_read(Bedrock *br, u8 port);
     Signal dev_write(Bedrock *br, u8 port, u8 val);
diff --git a/arm9/source/main_debug.c b/arm9/source/debug.c
index eae5192..eae5192 100644
--- a/arm9/source/main_debug.c
+++ b/arm9/source/debug.c
diff --git a/arm9/source/main_debug.h b/arm9/source/debug.h
index 55e60b9..36bfca3 100644
--- a/arm9/source/main_debug.h
+++ b/arm9/source/debug.h
@@ -1,5 +1,5 @@
-#ifndef MAIN_DEBUG_H_
-    #define MAIN_DEBUG_H_
+#ifndef DEBUG_H_
+    #define DEBUG_H_
 
     void debug_stacks(Bedrock *br);
     void debug_assert(Bedrock *br);
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);
 }
diff --git a/arm9/source/main.c b/arm9/source/main.c
index a8d3273..8bb78d8 100644
--- a/arm9/source/main.c
+++ b/arm9/source/main.c
@@ -6,18 +6,20 @@
 #include "devices/screen.h"
 #include "bang.h"
 #include "core.h"
-#include "main_debug.h"
 
-Bedrock br[5] = {};
+#define NUM_BR 5
+
+Bedrock br[NUM_BR] = {};
 Bedrock *br_main;
 Bedrock *br_sub;
 bool main_on_bottom = TRUE;
 
+
 u8 main_program[] = {
-    #include "../include/sysinfo.br.inc"
+    #include "../include/cobalt.br.inc"
 };
 u8 keyboard_program[] = {
-    #include "../include/sysinfo.br.inc"
+    00 //#include "../include/sysinfo.br.inc"
 };
 
 // Change to the next screen layout.
@@ -33,18 +35,6 @@ void receive_input(void) {
 }
 
 
-void run_br(Bedrock *br) {
-    if (br->awake) {
-        switch (evaluate(br, 1000)) {
-        case SIG_HALT:   br->alive = FALSE; black_screen(br->scr.nds); br = NULL; return;
-        case SIG_SLEEP:  br->awake = FALSE;            return;
-        case SIG_DB1:     debug_stacks(br);            return;
-        case SIG_DB4:     debug_assert(br);            return;
-        default:                                       return;
-        }
-    }
-}
-
 Screen scr_main = {
     .bgv = BG_TILE_RAM(BG_SLOT_VIS),
     .fgv = BG_TILE_RAM(FG_SLOT_VIS),
@@ -67,6 +57,11 @@ int main(void) {
     #define AWAKE(br)  (br && br->alive &&  br->awake)
     #define ASLEEP(br) (br && br->alive && !br->awake)
 
+    // Set memory identifiers.
+    for (int i=0; i<NUM_BR; i++) {
+        br[i].mem.id = i+1;
+    }
+
     init_screens();
     init_clock();
     lcdMainOnBottom();