From cb6f5a1c7ad38e09cb47d402b2b5d78fdd5965cc Mon Sep 17 00:00:00 2001
From: PikalaxALT <pikalaxalt@gmail.com>
Date: Fri, 20 Aug 2021 21:00:20 -0400
Subject: AllocFromHead, ALlocFromTail

---
 arm9/lib/libnns/src/NNS_FND_expheap.c | 65 +++++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)

(limited to 'arm9/lib/libnns/src')

diff --git a/arm9/lib/libnns/src/NNS_FND_expheap.c b/arm9/lib/libnns/src/NNS_FND_expheap.c
index f5a74dc4..31ea5fbe 100644
--- a/arm9/lib/libnns/src/NNS_FND_expheap.c
+++ b/arm9/lib/libnns/src/NNS_FND_expheap.c
@@ -36,6 +36,11 @@ static inline void* GetMBlockEndAddr(NNSiFndExpHeapMBlockHead* block)
     return AddU32ToPtr(GetMemPtrForMBlock(block), block->blockSize);
 }
 
+static inline u16 GetAllocMode(NNSiFndExpHeapHead* pExHeapHd)
+{
+    return NNSi_FndGetBitValue(pExHeapHd->feature, 0, 1);
+}
+
 static inline void SetAllocMode(NNSiFndExpHeapHead* pExHeapHd, u16 mode)
 {
     NNSi_FndSetBitValue(pExHeapHd->feature, 0, 1, mode);
@@ -202,3 +207,63 @@ void* AllocUsedBlockFromFreeBlock(NNSiFndExpHeapHead* pEHHead, NNSiFndExpHeapMBl
 
     return mblock;
 }
+
+void* AllocFromHead(NNSiFndHeapHead* pHeapHd, u32 size, int alignment)
+{
+    NNSiFndExpHeapHead* pExpHeapHd = GetExpHeapHeadPtrFromHeapHead(pHeapHd);
+    const BOOL bAllocFirst = GetAllocMode(pExpHeapHd) == 0;
+    NNSiFndExpHeapMBlockHead* pMBlkHd = NULL;
+    NNSiFndExpHeapMBlockHead* pMBlkHdFound = NULL;
+    u32 foundSize = 0xFFFFFFFF;
+    void* foundMBlock = NULL;
+
+    for (pMBlkHd = pExpHeapHd->mbFreeList.head; pMBlkHd; pMBlkHd = pMBlkHd->pMBHeadNext)
+    {
+        void *const mblock = GetMemPtrForMBlock(pMBlkHd);
+        void *const reqMBlock = NNSi_FndRoundUpPtr(mblock, alignment);
+        const u32 offset = GetOffsetFromPtr(mblock, reqMBlock);
+        if (pMBlkHd->blockSize >= size + offset && foundSize > pMBlkHd->blockSize)
+        {
+            pMBlkHdFound = pMBlkHd;
+            foundSize = pMBlkHd->blockSize;
+            foundMBlock = reqMBlock;
+            if (bAllocFirst || foundSize == size)
+                break;
+        }
+    }
+
+    if (!pMBlkHdFound)
+        return NULL;
+
+    return AllocUsedBlockFromFreeBlock(pExpHeapHd, pMBlkHdFound, foundMBlock, size, 0);
+}
+
+void* AllocFromTail(NNSiFndHeapHead* pHeapHd, u32 size, int alignment)
+{
+    NNSiFndExpHeapHead* pExpHeapHd = GetExpHeapHeadPtrFromHeapHead(pHeapHd);
+    const BOOL bAllocFirst = GetAllocMode(pExpHeapHd) == 0;
+    NNSiFndExpHeapMBlockHead* pMBlkHd = NULL;
+    NNSiFndExpHeapMBlockHead* pMBlkHdFound = NULL;
+    u32 foundSize = 0xFFFFFFFF;
+    void* foundMBlock = NULL;
+
+    for (pMBlkHd = pExpHeapHd->mbFreeList.tail; pMBlkHd; pMBlkHd = pMBlkHd->pMBHeadPrev)
+    {
+        void *const mblock = GetMemPtrForMBlock(pMBlkHd);
+        void *const mBlockEnd = AddU32ToPtr(mblock, pMBlkHd->blockSize);
+        void *const reqMBlock = NNSi_FndRoundDownPtr(SubU32ToPtr(mBlockEnd, size), alignment);
+        if (ComparePtr(reqMBlock, mblock) >= 0 && foundSize > pMBlkHd->blockSize)
+        {
+            pMBlkHdFound = pMBlkHd;
+            foundSize = pMBlkHd->blockSize;
+            foundMBlock = reqMBlock;
+            if (bAllocFirst || foundSize == size)
+                break;
+        }
+    }
+
+    if (!pMBlkHdFound)
+        return NULL;
+
+    return AllocUsedBlockFromFreeBlock(pExpHeapHd, pMBlkHdFound, foundMBlock, size, 1);
+}
-- 
cgit v1.2.3