THeap: Add `THeap` class

Some matching issues, but still a good start.

1 files changed, 190 insertions, 0 deletions

diff --git a/src/pso/THeap.cpp b/src/pso/THeap.cpp
new file mode 100644
index 0000000..87598b9
--- /dev/null
+++ b/src/pso/THeap.cpp
@@ -0,0 +1,190 @@
+#include "pso/THeap.h"
+#include <stdlib.h>
+#include <string.h>
+#include <global_types.h>
+
+#undef MATCHING
+#define MATCHING
+
+static const int heap_offset = 8;
+
+void xfree(void *ptr) {
+
+}
+
+void *xmalloc(size_t size) {
+	return NULL;
+}
+
+void THeap::heap_free(void *ptr) {
+	#define heap_ptr &ptr_u8[-heap_offset]
+	#define ptr_heap ((THeap *)heap_ptr)
+	const u8 *ptr_u8 = (u8 *)ptr;
+	if (ptr == NULL) {
+		return;
+	} else {
+		THeap *parent = (THeap *)heap;
+		THeap *child;
+
+		for (; child = (THeap *)parent->heap, child < ptr_heap; parent = child);
+
+		if (((u8 *)&ptr_heap->heap + ptr_heap->alloc_size) == (u8 *)&child->heap) {
+			ptr_heap->heap = child->heap;
+			ptr_heap->alloc_size += child->alloc_size;
+		} else {
+			ptr_heap->heap = (u8 *)child;
+		}
+
+		if (((u8 *)&parent->heap + parent->alloc_size) == heap_ptr) {
+			parent->heap = ptr_heap->heap;
+			parent->alloc_size += ptr_heap->alloc_size;
+		} else {
+			parent->heap = (u8 *)heap_ptr;
+		}
+	}
+	#undef ptr_heap
+	#undef heap_ptr
+}
+
+void *THeap::heap_zalloc(size_t size) {
+	void *ptr = heap_alloc(size);
+	if (ptr != NULL) {
+		memset(ptr, 0, size);
+	}
+	return ptr;
+}
+
+// Getting closer, but register allocation issues.
+#ifndef MATCHING
+void *THeap::heap_alloc(size_t size) {
+	const u32 aligned_size = ((heap_offset - 1) + align + size) & -align;
+	THeap *parent = (THeap *)heap;
+	THeap *child;
+	goto start;
+	cond:
+	if (child->alloc_size < aligned_size) {
+		goto loop_body;
+	}
+
+	top:
+	if (aligned_size == child->alloc_size) {
+		parent->heap = child->heap;
+	} else {
+		THeap *tmp = (THeap *)&child->heap[aligned_size];
+		tmp->heap = child->heap;
+		tmp->alloc_size = child->alloc_size - aligned_size;
+		child->alloc_size = aligned_size;
+		parent->heap = (u8 *)tmp;
+	}
+	return (void *)&child->align;
+
+	loop_body:
+	parent = child;
+	start:
+	child = (THeap *)parent->heap;
+	if (child == NULL) {
+		return NULL;
+	}
+	goto cond;
+}
+#else
+asm void *THeap::heap_alloc(size_t size) {
+nofralloc
+/* 80141028 80030008 */ lwz         r0, 8(r3)
+/* 8014102C 80630000 */ lwz         r3, 0(r3)
+/* 80141030 7CC02214 */ add         r6, r0, r4
+/* 80141034 7C0000D0 */ neg         r0, r0
+/* 80141038 38C60007 */ addi        r6, r6, 0x7
+/* 8014103C 7CC60038 */ and         r6, r6, r0
+/* 80141040 48000050 */ b           lbl_80141090
+lbl_80141044:
+/* 80141044 80040004 */ lwz         r0, 4(r4)
+/* 80141048 7C003040 */ cmplw       r0, r6
+/* 8014104C 41800040 */ blt-        lbl_8014108c
+/* 80141050 7C060040 */ cmplw       r6, r0
+/* 80141054 40820010 */ bne-        lbl_80141064
+/* 80141058 80040000 */ lwz         r0, 0(r4)
+/* 8014105C 90030000 */ stw         r0, 0(r3)
+/* 80141060 48000024 */ b           lbl_80141084
+lbl_80141064:
+/* 80141064 80040000 */ lwz         r0, 0(r4)
+/* 80141068 7CA43214 */ add         r5, r4, r6
+/* 8014106C 90050000 */ stw         r0, 0(r5)
+/* 80141070 80040004 */ lwz         r0, 4(r4)
+/* 80141074 7C060050 */ subf        r0, r6, r0
+/* 80141078 90050004 */ stw         r0, 4(r5)
+/* 8014107C 90C40004 */ stw         r6, 4(r4)
+/* 80141080 90A30000 */ stw         r5, 0(r3)
+lbl_80141084:
+/* 80141084 38640008 */ addi        r3, r4, 0x8
+/* 80141088 4E800020 */ blr
+lbl_8014108c:
+/* 8014108C 7C832378 */ mr          r3, r4
+lbl_80141090:
+/* 80141090 80830000 */ lwz         r4, 0(r3)
+/* 80141094 28040000 */ cmplwi      r4, 0
+/* 80141098 4082FFAC */ bne+        lbl_80141044
+/* 8014109C 38600000 */ li          r3, 0x0
+/* 801410A0 4E800020 */ blr
+}
+#endif
+
+THeap::~THeap() {
+	xfree(heap);
+}
+
+// Not sure how to get this matching.
+#ifndef MATCHING
+THeap::THeap(size_t size, int alignment) {
+	heap_size = 0;
+	mbr_0x10 = 0;
+	align = alignment;
+	heap = (u8 *)xmalloc(heap_size);
+	if (heap != NULL) {
+		THeap *tmp_heap = (THeap *)memset(heap, 0, size);
+		tmp_heap->heap = &heap[heap_offset];
+		((THeap *)heap)->alloc_size = 0;
+		tmp_heap->align = 0;
+		tmp_heap->heap_size = size - heap_offset;
+	}
+}
+#else
+asm THeap::THeap(size_t size, int alignment) {
+nofralloc
+/* 801410F0 9421FFF0 */ stwu        r1, -0x10(r1)
+/* 801410F4 7C0802A6 */ mflr        r0
+/* 801410F8 90010014 */ stw         r0, 0x14(r1)
+/* 801410FC 38000000 */ li          r0, 0x0
+/* 80141100 BFC10008 */ stmw        r30, 8(r1)
+/* 80141104 7C7E1B78 */ mr          r30, r3
+/* 80141108 7C9F2378 */ mr          r31, r4
+/* 8014110C 9003000C */ stw         r0, 0xc(r3)
+/* 80141110 7FE3FB78 */ mr          r3, r31
+/* 80141114 901E0010 */ stw         r0, 0x10(r30)
+/* 80141118 90BE0008 */ stw         r5, 8(r30)
+/* 8014111C 48299FC5 */ bl          xmalloc
+/* 80141120 907E0000 */ stw         r3, 0(r30)
+/* 80141124 807E0000 */ lwz         r3, 0(r30)
+/* 80141128 28030000 */ cmplwi      r3, 0
+/* 8014112C 41820034 */ beq-        lbl_80141160
+/* 80141130 7FE5FB78 */ mr          r5, r31
+/* 80141134 38800000 */ li          r4, 0x0
+/* 80141138 4BECD1FD */ bl          memset
+/* 8014113C 807E0000 */ lwz         r3, 0(r30)
+/* 80141140 38800000 */ li          r4, 0x0
+/* 80141144 381FFFF8 */ addi        r0, r31, -0x8
+/* 80141148 38A30008 */ addi        r5, r3, 0x8
+/* 8014114C 90A30000 */ stw         r5, 0(r3)
+/* 80141150 807E0000 */ lwz         r3, 0(r30)
+/* 80141154 90830004 */ stw         r4, 4(r3)
+/* 80141158 90850000 */ stw         r4, 0(r5)
+/* 8014115C 90050004 */ stw         r0, 4(r5)
+lbl_80141160:
+/* 80141160 7FC3F378 */ mr          r3, r30
+/* 80141164 BBC10008 */ lmw         r30, 8(r1)
+/* 80141168 80010014 */ lwz         r0, 0x14(r1)
+/* 8014116C 7C0803A6 */ mtlr        r0
+/* 80141170 38210010 */ addi        r1, r1, 0x10
+/* 80141174 4E800020 */ blr
+}
+#endif