1 files changed, 138 insertions, 93 deletions

diff --git a/src/SDK/OS/OSAlloc.c b/src/SDK/OS/OSAlloc.c
index 8422ab5..9270776 100644
--- a/src/SDK/OS/OSAlloc.c
+++ b/src/SDK/OS/OSAlloc.c
@@ -1,6 +1,48 @@
 #include "types.h"
 #include "OS/OSAlloc.h"
 
+// TODO: move to header
+typedef enum {
+    OS_ARENA_MAIN = 0,
+    OS_ARENA_MAIN_SUBPRIV = 1,
+    OS_ARENA_MAINEX = 2,
+    OS_ARENA_ITCM = 3,
+    OS_ARENA_DTCM = 4,
+    OS_ARENA_SHARED = 5,
+    OS_ARENA_WRAM_MAIN = 6,
+    OS_ARENA_WRAM_SUB = 7,
+    OS_ARENA_WRAM_SUBPRIV = 8,
+    OS_ARENA_MAX = 9
+} OSArenaId;
+
+#define OSi_ROUND(n, a)            (((u32) (n) + (a) - 1) & ~((a) - 1))
+
+// NOTE: there is just one HeapInfo pointer in PBR, not an array
+// void* OSiHeapInfo[OS_ARENA_MAX];
+
+inline Cell* DLAddFront(Cell* list, Cell* cell)
+{
+    cell->next = list;
+    cell->prev = NULL;
+
+    if (list != NULL)
+        list->prev = cell;
+    return cell;
+}
+
+inline Cell* DLExtract(Cell* list, Cell* cell)
+{
+    if (cell->next) {
+        cell->next->prev = cell->prev;
+    }
+    if (cell->prev == NULL) {
+        list = cell->next;
+    } else {
+        cell->prev->next = cell->next;
+    }
+    return list;
+}
+
 Cell *DLInsert(Cell *original, Cell *inserted)
 {
     Cell *prevCell = NULL;
@@ -48,18 +90,6 @@ Cell *DLInsert(Cell *original, Cell *inserted)
     return inserted;
 }
 
-// sizeof unkStruct == 12
-// element of a "heap array"
-typedef struct unkStruct {
-    s32 heapSize; // initialized to -1 in OSInitAlloc, set to heap size in OSCreateHeap
-    Cell *start; // set to start param of OSCreateHeap
-    Cell *freeList; // initialized to NULL in OSInitAlloc along with start
-                // head of a linked list of allocated blocks (the free list)
-} unkStruct;
-
-// arenaStart param of OSInitAlloc, points to array of maxHeap unkStructs, 68c
-static unkStruct *gUnk8063fa78; 
-
 #if 0 // not found in DOL
 static u32 gUnk8063fa6c; // aligned arenaEnd, 680
 
@@ -74,16 +104,16 @@ volatile OSHeapHandle __OSCurrHeap = -1;
 
 void *OSInitAlloc(void *arenaStart, void *arenaEnd, int maxHeaps)
 {
-    gUnk8063fa78 = (unkStruct *)arenaStart;
+    HeapArray = (HeapDesc *)arenaStart;
     gUnk8063fa74 = maxHeaps;
     for (int i = 0; i < gUnk8063fa74; i++) {
-        gUnk8063fa78[i].heapSize = -1; // -1 indicates this heap is unused
-        gUnk8063fa78[i].start = NULL;
-        gUnk8063fa78[i].freeList = NULL;
+        HeapArray[i].heapSize = -1; // -1 indicates this heap is unused
+        HeapArray[i].start = NULL;
+        HeapArray[i].freeList = NULL;
     }
     gUnk8063fa6c = (u32)arenaEnd & ~0x1f;
     __OSCurrHeap = -1;
-    gUnk8063fa70 = (void *)(((u32)&gUnk8063fa78[maxHeaps] + 0x1f) & ~0x1f);
+    gUnk8063fa70 = (void *)(((u32)&HeapArray[maxHeaps] + 0x1f) & ~0x1f);
     return gUnk8063fa70;
 }
 
@@ -92,13 +122,13 @@ OSHeapHandle OSCreateHeap(void *start, void *end)
     start = (void *)((u32)start + 0x1f & ~0x1f);
     end = (void *)((u32)end & ~0x1f);
     for (int i = 0; i < gUnk8063fa74; i++) {
-        if (gUnk8063fa78[i].heapSize < 0) {
-            gUnk8063fa78[i].heapSize = end - start;
+        if (HeapArray[i].heapSize < 0) {
+            HeapArray[i].heapSize = end - start;
             ((Cell *)start)->prev = NULL;
             ((Cell *)start)->next = NULL;
-            ((Cell *)start)->size = gUnk8063fa78[i].heapSize;
-            gUnk8063fa78[i].start = (Cell *)start;
-            gUnk8063fa78[i].freeList = NULL;
+            ((Cell *)start)->size = HeapArray[i].heapSize;
+            HeapArray[i].start = (Cell *)start;
+            HeapArray[i].freeList = NULL;
             return i;
         }
     }
@@ -107,84 +137,99 @@ OSHeapHandle OSCreateHeap(void *start, void *end)
 
 #endif
 
-// NOTE: matches except for r4/r5 reg swap
-void *OSAllocFromHeap(OSHeapHandle heap, u32 size)
-{
-    // r4, add 32 bytes to requested size (for the header), then round up
-    // to the next multiple of 32 (for alignment)
-    //s32 sizeWithHeader = size + 32;
-    unkStruct *r5 = &gUnk8063fa78[heap]; // r5, start parameter from OSInitAlloc
-    Cell *r3 = r5->start;
-    s32 totalSize = ((size + 63) & ~0x1f); // r4
-    Cell *r6;
-    for (r6 = r3; r6; r6 = r6->next) {
-        if (totalSize <= r6->size) break;
+// arenaStart param of OSInitAlloc, points to array of
+// maxHeap HeapDescs, 68c
+HeapDesc *HeapArray; 
+
+#define HEADERSIZE OSi_ROUND(sizeof(Cell), 32)
+#define MINOBJSIZE (HEADERSIZE+32)
+
+void* OSAllocFromHeap(/*OSArenaId id,*/ OSHeapHandle heap, u32 size) {
+    // OSHeapInfo* heapInfo;
+    HeapDesc* hd;
+    Cell* cell;
+    Cell* newCell;
+    long leftoverSize;
+    
+    /*
+    OSIntrMode enabled = OS_DisableInterrupts();
+    heapInfo = OSiHeapInfo[id];
+    if (!heapInfo) {
+        (void)OS_RestoreInterrupts(enabled);
+        return NULL;
+    }
+
+    if (heap < 0) {
+        heap = heapInfo->currentHeap;
     }
+    */
     
-    if (!r6) return NULL;
+    //hd = &heapInfo->heapArray[heap];
+    hd = &HeapArray[heap];
     
-    // r6 has enough space to satisfy the request
-    u32 difference = r6->size - totalSize; // r0
-    if (difference < 0x40) {
-        // this heap has reached its memory limit, so
-        // remove r6 node from doubly linked list
-        Cell *nextCell = r6->next;
-        if (nextCell)
-            nextCell->prev = r6->prev;
-        Cell *prevCell = r6->prev;
-        if (!prevCell)
-            r3 = r6->next;
-        else
-            prevCell->next = r6->next;
-        r5->start = r3;
-    } else {
-        // REPLACE r6 node with r6SizeShifted node in the
-        // linked list. Set the new node's size to be the
-        // amount of bytes remaining
-        r6->size = totalSize;
-        Cell *r6SizeShifted = (Cell *)((u32)r6 + totalSize); // r4
-        r6SizeShifted->size = difference;
-        r6SizeShifted->prev = r6->prev;
-        Cell *r6Next = r6->next;
-        r6SizeShifted->next = r6Next;
-        if (r6Next) {
-            r6Next->prev = r6SizeShifted;
+    size += HEADERSIZE;
+    size = OSi_ROUND(size, 32);
+
+    for (cell = hd->free; cell != NULL; cell = cell->next) {
+        if ((long)size <= cell->size) {
+            break;
         }
-        // connect doubly linked list
-        if (r6SizeShifted->prev)
-            r6SizeShifted->prev->next = r6SizeShifted;
-        else
-           r5->start = r6SizeShifted;
     }
-    // Make the r6 node the new head of another linked list
-    // headed by r5->freeList
-    Cell *fList = r5->freeList;
-    r6->next = fList;
-    r6->prev = NULL;
-    if (fList) {
-        fList->prev = r6;
-    }
-    r5->freeList = r6;
-    return (void*)((u32)r6 + 32);
-}
 
-void OSFreeToHeap(OSHeapHandle heap, void *ptr)
-{
-    // heap block has a 32-byte header preceding the real data
-    Cell *node = (Cell *)((u32)ptr - 32); // r4
-    unkStruct *r31 = &gUnk8063fa78[heap];
-    Cell *r3 = r31->freeList;
-    // removing node from freeList
-    Cell *r6 = node->next;
-    if (r6) {
-        r6->prev = node->prev;
+    if (cell == NULL) {
+        //(void)OS_RestoreInterrupts(enabled);
+        return NULL;
     }
-    Cell *r5 = node->prev;
-    if (!r5) {
-        r3 = node->next;
+
+    leftoverSize = cell->size - (long)size;
+    if (leftoverSize < MINOBJSIZE) {
+        hd->free = DLExtract(hd->free, cell);
     } else {
-        r5->next = node->next;
+        cell->size = (long)size;
+
+        newCell = (Cell *) ((char *)cell + size);
+        newCell->size = leftoverSize;
+
+        newCell->prev = cell->prev;
+        newCell->next = cell->next;
+
+        if (newCell->next != NULL) {
+            newCell->next->prev = newCell;
+        }
+
+        if (newCell->prev != NULL) {
+            newCell->prev->next = newCell;
+        } else {
+            hd->free = newCell;
+        }
     }
-    r31->freeList = r3; // new head
-    r31->start = DLInsert(r31->start, node);
+
+    hd->allocated = DLAddFront(hd->allocated, cell);
+
+    //(void)OS_RestoreInterrupts(enabled);
+    return (void *)((char *)cell + HEADERSIZE);
+}
+
+void OSFreeToHeap(/*OSArenaId id,*/ OSHeapHandle heap, void* ptr) {
+    OSHeapInfo *heapInfo;
+    HeapDesc *hd;
+    Cell   *cell;
+    
+    /*
+    OSIntrMode enabled = OS_DisableInterrupts();
+    heapInfo = OSiHeapInfo[id];
+
+    if (heap < 0) {
+        heap = heapInfo->currentHeap;
+    }
+    */
+
+    cell = (Cell *) ((char *)ptr - HEADERSIZE);
+    //hd = &heapInfo->heapArray[heap];
+    hd = &HeapArray[heap];
+    
+    hd->allocated = DLExtract(hd->allocated, cell);
+    hd->free = DLInsert(hd->free, cell);
+
+    //(void)OS_RestoreInterrupts(enabled);
 }