From 562cc896a1142735163bfeac7dd29ec86e9140f2 Mon Sep 17 00:00:00 2001
From: fincs <fincs.alt1@gmail.com>
Date: Wed, 5 Nov 2014 20:58:50 +0100
Subject: [PATCH] Add linearMemAlign() (current implementation sucks)

---
 libctru/include/3ds/linear.h        |  5 +++--
 libctru/source/allocator/linear.cpp | 31 ++++++++++++++++++++++++-----
 2 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/libctru/include/3ds/linear.h b/libctru/include/3ds/linear.h
index ae4c915..8fe2c7d 100644
--- a/libctru/include/3ds/linear.h
+++ b/libctru/include/3ds/linear.h
@@ -1,7 +1,8 @@
 #pragma once
 
 // Functions for allocating/deallocating memory from linear heap
-void* linearAlloc(size_t size);
-void* linearRealloc(void* mem, size_t size);
+void* linearAlloc(size_t size); // returns a 16-byte aligned address
+void* linearMemAlign(size_t size, size_t alignment); // WARNING: wastes 'alignment' bytes
+void* linearRealloc(void* mem, size_t size); // not implemented yet
 void linearFree(void* mem);
 u32 linearSpaceFree(); // get free linear space in bytes
diff --git a/libctru/source/allocator/linear.cpp b/libctru/source/allocator/linear.cpp
index 83d1143..0a47275 100644
--- a/libctru/source/allocator/linear.cpp
+++ b/libctru/source/allocator/linear.cpp
@@ -16,24 +16,44 @@ static bool linearInit()
 	return false;
 }
 
-void* linearAlloc(size_t size)
+void* linearMemAlign(size_t size, size_t alignment)
 {
+	// Enforce minimum alignment
+	if (alignment < 16)
+		alignment = 16;
+
+	// Convert alignment to shift amount
+	int shift;
+	for (shift = 4; shift < 32; shift ++)
+	{
+		if ((1U<<shift) == alignment)
+			break;
+	}
+	if (shift == 32) // Invalid alignment
+		return nullptr;
+
 	// Initialize the pool if it is not ready
 	if (!sLinearPool.Ready() && !linearInit())
 		return nullptr;
 
 	// Reserve memory for MemChunk structure
-	size += 16;
+	size += alignment;
 
 	// Allocate the chunk
 	MemChunk chunk;
-	if (!sLinearPool.Allocate(chunk, size, 4)) // 16-byte alignment
+	if (!sLinearPool.Allocate(chunk, size, shift))
 		return nullptr;
 
 	// Copy the MemChunk structure and return memory
 	auto addr = chunk.addr;
 	*(MemChunk*)addr = chunk;
-	return addr + 16;
+	*(u32*)(addr + alignment - sizeof(u32)) = alignment;
+	return addr + alignment;
+}
+
+void* linearAlloc(size_t size)
+{
+	return linearMemAlign(size, 16);
 }
 
 void* linearRealloc(void* mem, size_t size)
@@ -45,7 +65,8 @@ void* linearRealloc(void* mem, size_t size)
 void linearFree(void* mem)
 {
 	// Find MemChunk structure and free the chunk
-	auto pChunk = (MemChunk*)((u8*)mem - 16);
+	u32 alignment = *((u32*)mem - 1);
+	auto pChunk = (MemChunk*)((u8*)mem - alignment);
 	sLinearPool.Deallocate(*pChunk);
 }
 
-- 
2.39.5