2 files changed, 82 insertions, 0 deletions
diff --git a/arm9/lib/libnns/include/NNS_FND_heapcommon.h b/arm9/lib/libnns/include/NNS_FND_heapcommon.h
index 22a6bfdf..77473efe 100644
--- a/arm9/lib/libnns/include/NNS_FND_heapcommon.h
+++ b/arm9/lib/libnns/include/NNS_FND_heapcommon.h
@@ -2,9 +2,12 @@
 #define GUARD_NNS_FND_HEAPCOMMON_H
 
 #include "NNS_FND_list.h"
+#include "MI_memory.h"
 
 #define NNS_FND_HEAP_DEFAULT_ALIGNMENT          4
 
+#define NNS_FndGetFillValForHeap(type) (0)
+
 typedef struct NNSiFndHeapHead NNSiFndHeapHead;
 
 typedef s32 NNSiIntPtr;
@@ -87,6 +90,12 @@ static inline void SetOptForHeap(
     NNSi_FndSetBitValue(pHeapHd->attribute, 0, 8, optFlag);
 }
 
+static inline void FillAllocMemory(NNSiFndHeapHead* pHeapHd, void* address, u32 size)
+{
+    if (GetOptForHeap(pHeapHd) & 1)
+        MI_CpuFill32(address, NNS_FndGetFillValForHeap(0), size);
+}
+
 void NNSi_FndInitHeapHead(NNSiFndHeapHead *pHead, u32 signature, void* heapStart, void* heapEnd, u16 optionFlag);
 
 #endif //GUARD_NNS_FND_HEAPCOMMON_H
diff --git a/arm9/lib/libnns/src/NNS_FND_expheap.c b/arm9/lib/libnns/src/NNS_FND_expheap.c
index eee4f9ca..f5a74dc4 100644
--- a/arm9/lib/libnns/src/NNS_FND_expheap.c
+++ b/arm9/lib/libnns/src/NNS_FND_expheap.c
@@ -11,6 +11,21 @@ static inline u16 GetAlignmentForMBlock(NNSiFndExpHeapMBlockHead* block)
     return NNSi_FndGetBitValue(block->attribute, 8, 7);
 }
 
+static inline void SetAllocDirForMBlock(NNSiFndExpHeapMBlockHead* pEHMBHead, u16 direction)
+{
+    NNSi_FndSetBitValue(pEHMBHead->attribute, 15, 1, direction);
+}
+
+static inline void SetAlignmentForMBlock(NNSiFndExpHeapMBlockHead* pEHMBHead, u16 alignment)
+{
+    NNSi_FndSetBitValue(pEHMBHead->attribute, 8, 7, alignment);
+}
+
+static inline void SetGroupIDForMBlock(NNSiFndExpHeapMBlockHead* pEHMBHead, u16 groupID)
+{
+    NNSi_FndSetBitValue(pEHMBHead->attribute, 0, 8, groupID);
+}
+
 static inline void* GetMemPtrForMBlock(NNSiFndExpHeapMBlockHead* block)
 {
     return AddU32ToPtr(block, sizeof(NNSiFndExpHeapMBlockHead));
@@ -31,6 +46,11 @@ static inline NNSiFndExpHeapHead* GetExpHeapHeadPtrFromHeapHead(NNSiFndHeapHead*
     return AddU32ToPtr(pHHead, sizeof(NNSiFndHeapHead));
 }
 
+static inline NNSiFndHeapHead* GetHeapHeadPtrFromExpHeapHead(NNSiFndExpHeapHead* pEHHead)
+{
+    return SubU32ToPtr(pEHHead, sizeof(NNSiFndHeapHead));
+}
+
 void GetRegionOfMBlock(NNSiMemRegion* region, NNSiFndExpHeapMBlockHead* block)
 {
     region->start = SubU32ToPtr(block, GetAlignmentForMBlock(block));
@@ -129,3 +149,56 @@ NNSiFndHeapHead* InitExpHeap(void* startAddress, void* endAddress, u16 optFlag)
 
     return pHeapHd;
 }
+
+static inline void AppendMBlock(NNSiFndExpMBlockList* list, NNSiFndExpHeapMBlockHead* block)
+{
+    (void) InsertMBlock(list, block, list->tail);
+}
+
+void* AllocUsedBlockFromFreeBlock(NNSiFndExpHeapHead* pEHHead, NNSiFndExpHeapMBlockHead* pMBHeadFree, void* mblock, u32 size, u16 direction)
+{
+    NNSiMemRegion freeRgnT;
+    NNSiMemRegion freeRgnB;
+    NNSiFndExpHeapMBlockHead* pMBHeadFreePrev;
+
+    GetRegionOfMBlock(&freeRgnT, pMBHeadFree);
+
+    freeRgnB.end = freeRgnT.end;
+    freeRgnB.start = AddU32ToPtr(mblock, size);
+    freeRgnT.end = SubU32ToPtr(mblock, sizeof(NNSiFndExpHeapMBlockHead));
+
+    pMBHeadFreePrev = RemoveMBlock(&pEHHead->mbFreeList, pMBHeadFree);
+
+    if (GetOffsetFromPtr(freeRgnT.start, freeRgnT.end) < sizeof(NNSiFndExpHeapMBlockHead))
+    {
+        freeRgnT.end = freeRgnT.start;
+    }
+    else
+    {
+        pMBHeadFreePrev = InsertMBlock(&pEHHead->mbFreeList, InitFreeMBlock(&freeRgnT), pMBHeadFreePrev);
+    }
+    if (GetOffsetFromPtr(freeRgnB.start, freeRgnB.end) < sizeof(NNSiFndExpHeapMBlockHead))
+    {
+        freeRgnB.start= freeRgnB.end;
+    }
+    else
+    {
+        (void)InsertMBlock(&pEHHead->mbFreeList, InitFreeMBlock(&freeRgnB), pMBHeadFreePrev);
+    }
+
+    FillAllocMemory(GetHeapHeadPtrFromExpHeapHead(pEHHead), freeRgnT.end, GetOffsetFromPtr(freeRgnT.end, freeRgnB.start));
+
+    NNSiFndExpHeapMBlockHead* pMBHeadNewUsed;
+    NNSiMemRegion region;
+
+    region.start = SubU32ToPtr(mblock, sizeof(NNSiFndExpHeapMBlockHead));
+    region.end = freeRgnB.start;
+
+    pMBHeadNewUsed = InitMBlock(&region, 0x5544);
+    SetAllocDirForMBlock(pMBHeadNewUsed, direction);
+    SetAlignmentForMBlock(pMBHeadNewUsed, (u16)GetOffsetFromPtr(freeRgnT.end, pMBHeadNewUsed));
+    SetGroupIDForMBlock(pMBHeadNewUsed, pEHHead->groupID);
+    AppendMBlock(&pEHHead->mbUsedList, pMBHeadNewUsed);
+
+    return mblock;
+}