OSFreeToHeap matching besides SDA offset, OSAllocFromHeap almost matching besides register swap

3 files changed, 148 insertions, 112 deletions

diff --git a/asm/SDK/OS/OSAlloc_asm.s b/asm/SDK/OS/OSAlloc_asm.s
index 2826b5c..47b234e 100644
--- a/asm/SDK/OS/OSAlloc_asm.s
+++ b/asm/SDK/OS/OSAlloc_asm.s
@@ -2,115 +2,3 @@
 
 .section .text, "ax"  # 0x80006980 - 0x803E1E60
 
-.global OSAllocFromHeap
-OSAllocFromHeap:
-/* 80269F5C 00265BBC  1C 63 00 0C */	mulli r3, r3, 0xc
-/* 80269F60 00265BC0  80 AD A7 B8 */	lwz r5, lbl_8063FA78-_SDA_BASE_(r13)
-/* 80269F64 00265BC4  38 04 00 3F */	addi r0, r4, 0x3f
-/* 80269F68 00265BC8  7C A5 1A 14 */	add r5, r5, r3
-/* 80269F6C 00265BCC  54 04 00 34 */	rlwinm r4, r0, 0, 0, 0x1a
-/* 80269F70 00265BD0  80 65 00 04 */	lwz r3, 4(r5)
-/* 80269F74 00265BD4  7C 66 1B 78 */	mr r6, r3
-/* 80269F78 00265BD8  48 00 00 14 */	b lbl_80269F8C
-lbl_80269F7C:
-/* 80269F7C 00265BDC  80 06 00 08 */	lwz r0, 8(r6)
-/* 80269F80 00265BE0  7C 04 00 00 */	cmpw r4, r0
-/* 80269F84 00265BE4  40 81 00 10 */	ble lbl_80269F94
-/* 80269F88 00265BE8  80 C6 00 04 */	lwz r6, 4(r6)
-lbl_80269F8C:
-/* 80269F8C 00265BEC  2C 06 00 00 */	cmpwi r6, 0
-/* 80269F90 00265BF0  40 82 FF EC */	bne lbl_80269F7C
-lbl_80269F94:
-/* 80269F94 00265BF4  2C 06 00 00 */	cmpwi r6, 0
-/* 80269F98 00265BF8  40 82 00 0C */	bne lbl_80269FA4
-/* 80269F9C 00265BFC  38 60 00 00 */	li r3, 0
-/* 80269FA0 00265C00  4E 80 00 20 */	blr
-lbl_80269FA4:
-/* 80269FA4 00265C04  80 06 00 08 */	lwz r0, 8(r6)
-/* 80269FA8 00265C08  7C 04 00 50 */	subf r0, r4, r0
-/* 80269FAC 00265C0C  28 00 00 40 */	cmplwi r0, 0x40
-/* 80269FB0 00265C10  40 80 00 3C */	bge lbl_80269FEC
-/* 80269FB4 00265C14  80 86 00 04 */	lwz r4, 4(r6)
-/* 80269FB8 00265C18  2C 04 00 00 */	cmpwi r4, 0
-/* 80269FBC 00265C1C  41 82 00 0C */	beq lbl_80269FC8
-/* 80269FC0 00265C20  80 06 00 00 */	lwz r0, 0(r6)
-/* 80269FC4 00265C24  90 04 00 00 */	stw r0, 0(r4)
-lbl_80269FC8:
-/* 80269FC8 00265C28  80 86 00 00 */	lwz r4, 0(r6)
-/* 80269FCC 00265C2C  2C 04 00 00 */	cmpwi r4, 0
-/* 80269FD0 00265C30  40 82 00 0C */	bne lbl_80269FDC
-/* 80269FD4 00265C34  80 66 00 04 */	lwz r3, 4(r6)
-/* 80269FD8 00265C38  48 00 00 0C */	b lbl_80269FE4
-lbl_80269FDC:
-/* 80269FDC 00265C3C  80 06 00 04 */	lwz r0, 4(r6)
-/* 80269FE0 00265C40  90 04 00 04 */	stw r0, 4(r4)
-lbl_80269FE4:
-/* 80269FE4 00265C44  90 65 00 04 */	stw r3, 4(r5)
-/* 80269FE8 00265C48  48 00 00 44 */	b lbl_8026A02C
-lbl_80269FEC:
-/* 80269FEC 00265C4C  90 86 00 08 */	stw r4, 8(r6)
-/* 80269FF0 00265C50  7C 86 22 14 */	add r4, r6, r4
-/* 80269FF4 00265C54  90 04 00 08 */	stw r0, 8(r4)
-/* 80269FF8 00265C58  80 06 00 00 */	lwz r0, 0(r6)
-/* 80269FFC 00265C5C  90 04 00 00 */	stw r0, 0(r4)
-/* 8026A000 00265C60  80 66 00 04 */	lwz r3, 4(r6)
-/* 8026A004 00265C64  2C 03 00 00 */	cmpwi r3, 0
-/* 8026A008 00265C68  90 64 00 04 */	stw r3, 4(r4)
-/* 8026A00C 00265C6C  41 82 00 08 */	beq lbl_8026A014
-/* 8026A010 00265C70  90 83 00 00 */	stw r4, 0(r3)
-lbl_8026A014:
-/* 8026A014 00265C74  80 64 00 00 */	lwz r3, 0(r4)
-/* 8026A018 00265C78  2C 03 00 00 */	cmpwi r3, 0
-/* 8026A01C 00265C7C  41 82 00 0C */	beq lbl_8026A028
-/* 8026A020 00265C80  90 83 00 04 */	stw r4, 4(r3)
-/* 8026A024 00265C84  48 00 00 08 */	b lbl_8026A02C
-lbl_8026A028:
-/* 8026A028 00265C88  90 85 00 04 */	stw r4, 4(r5)
-lbl_8026A02C:
-/* 8026A02C 00265C8C  80 65 00 08 */	lwz r3, 8(r5)
-/* 8026A030 00265C90  38 00 00 00 */	li r0, 0
-/* 8026A034 00265C94  90 66 00 04 */	stw r3, 4(r6)
-/* 8026A038 00265C98  2C 03 00 00 */	cmpwi r3, 0
-/* 8026A03C 00265C9C  90 06 00 00 */	stw r0, 0(r6)
-/* 8026A040 00265CA0  41 82 00 08 */	beq lbl_8026A048
-/* 8026A044 00265CA4  90 C3 00 00 */	stw r6, 0(r3)
-lbl_8026A048:
-/* 8026A048 00265CA8  90 C5 00 08 */	stw r6, 8(r5)
-/* 8026A04C 00265CAC  38 66 00 20 */	addi r3, r6, 0x20
-/* 8026A050 00265CB0  4E 80 00 20 */	blr
-
-.global OSFreeToHeap
-OSFreeToHeap:
-/* 8026A054 00265CB4  94 21 FF F0 */	stwu r1, -0x10(r1)
-/* 8026A058 00265CB8  7C 08 02 A6 */	mflr r0
-/* 8026A05C 00265CBC  38 84 FF E0 */	addi r4, r4, -32
-/* 8026A060 00265CC0  90 01 00 14 */	stw r0, 0x14(r1)
-/* 8026A064 00265CC4  1C 03 00 0C */	mulli r0, r3, 0xc
-/* 8026A068 00265CC8  93 E1 00 0C */	stw r31, 0xc(r1)
-/* 8026A06C 00265CCC  80 AD A7 B8 */	lwz r5, lbl_8063FA78-_SDA_BASE_(r13)
-/* 8026A070 00265CD0  80 C4 00 04 */	lwz r6, 4(r4)
-/* 8026A074 00265CD4  7F E5 02 14 */	add r31, r5, r0
-/* 8026A078 00265CD8  2C 06 00 00 */	cmpwi r6, 0
-/* 8026A07C 00265CDC  80 7F 00 08 */	lwz r3, 8(r31)
-/* 8026A080 00265CE0  41 82 00 0C */	beq lbl_8026A08C
-/* 8026A084 00265CE4  80 04 00 00 */	lwz r0, 0(r4)
-/* 8026A088 00265CE8  90 06 00 00 */	stw r0, 0(r6)
-lbl_8026A08C:
-/* 8026A08C 00265CEC  80 A4 00 00 */	lwz r5, 0(r4)
-/* 8026A090 00265CF0  2C 05 00 00 */	cmpwi r5, 0
-/* 8026A094 00265CF4  40 82 00 0C */	bne lbl_8026A0A0
-/* 8026A098 00265CF8  80 64 00 04 */	lwz r3, 4(r4)
-/* 8026A09C 00265CFC  48 00 00 0C */	b lbl_8026A0A8
-lbl_8026A0A0:
-/* 8026A0A0 00265D00  80 04 00 04 */	lwz r0, 4(r4)
-/* 8026A0A4 00265D04  90 05 00 04 */	stw r0, 4(r5)
-lbl_8026A0A8:
-/* 8026A0A8 00265D08  90 7F 00 08 */	stw r3, 8(r31)
-/* 8026A0AC 00265D0C  80 7F 00 04 */	lwz r3, 4(r31)
-/* 8026A0B0 00265D10  4B FF FE 01 */	bl DLInsert
-/* 8026A0B4 00265D14  90 7F 00 04 */	stw r3, 4(r31)
-/* 8026A0B8 00265D18  83 E1 00 0C */	lwz r31, 0xc(r1)
-/* 8026A0BC 00265D1C  80 01 00 14 */	lwz r0, 0x14(r1)
-/* 8026A0C0 00265D20  7C 08 03 A6 */	mtlr r0
-/* 8026A0C4 00265D24  38 21 00 10 */	addi r1, r1, 0x10
-/* 8026A0C8 00265D28  4E 80 00 20 */	blr
diff --git a/include/SDK/OS/OSAlloc.h b/include/SDK/OS/OSAlloc.h
index 150a5f8..10edb01 100644
--- a/include/SDK/OS/OSAlloc.h
+++ b/include/SDK/OS/OSAlloc.h
@@ -6,6 +6,11 @@ extern "C" {
 #endif
 
 typedef struct Cell Cell;
+typedef int  OSHeapHandle;
+
+#if 0
+extern volatile OSHeapHandle __OSCurrHeap; // used in OSInitAlloc
+#endif
 
 struct Cell {
     Cell* prev;
@@ -14,6 +19,8 @@ struct Cell {
 };
 
 Cell *DLInsert(Cell *original, Cell *inserted);
+void *OSAllocFromHeap(OSHeapHandle heap, u32 size);
+void OSFreeToHeap(OSHeapHandle heap, void *ptr);
 
 #ifdef __cplusplus
 }
diff --git a/src/SDK/OS/OSAlloc.c b/src/SDK/OS/OSAlloc.c
index 090233e..8422ab5 100644
--- a/src/SDK/OS/OSAlloc.c
+++ b/src/SDK/OS/OSAlloc.c
@@ -47,3 +47,144 @@ 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
+
+// returned by OSInitAlloc, "base address of the new arena,"
+// pointer to the beginning of the real data after the heap array header, 684
+static void *gUnk8063fa70;
+
+// Table of heaps
+static int gUnk8063fa74; // maxHeaps param of OSInitAlloc, 688
+
+volatile OSHeapHandle __OSCurrHeap = -1;
+
+void *OSInitAlloc(void *arenaStart, void *arenaEnd, int maxHeaps)
+{
+    gUnk8063fa78 = (unkStruct *)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;
+    }
+    gUnk8063fa6c = (u32)arenaEnd & ~0x1f;
+    __OSCurrHeap = -1;
+    gUnk8063fa70 = (void *)(((u32)&gUnk8063fa78[maxHeaps] + 0x1f) & ~0x1f);
+    return gUnk8063fa70;
+}
+
+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;
+            ((Cell *)start)->prev = NULL;
+            ((Cell *)start)->next = NULL;
+            ((Cell *)start)->size = gUnk8063fa78[i].heapSize;
+            gUnk8063fa78[i].start = (Cell *)start;
+            gUnk8063fa78[i].freeList = NULL;
+            return i;
+        }
+    }
+    return -1;
+}
+
+#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;
+    }
+    
+    if (!r6) return NULL;
+    
+    // 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;
+        }
+        // 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;
+    }
+    Cell *r5 = node->prev;
+    if (!r5) {
+        r3 = node->next;
+    } else {
+        r5->next = node->next;
+    }
+    r31->freeList = r3; // new head
+    r31->start = DLInsert(r31->start, node);
+}