Add aligned_static functions to Memory

(cherry picked from commit godotengine/godot@364f916)

Author: darksylinc

core/os/memory.cpp

@@ -37,6 +37,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
 
 void *operator new(size_t p_size, const char *p_description) {
 	return Memory::alloc_static(p_size, false);
@@ -67,6 +68,38 @@ SafeNumeric<uint64_t> Memory::max_usage;
 
 SafeNumeric<uint64_t> Memory::alloc_count;
 
+inline bool is_power_of_2(size_t x) { return x && ((x & (x - 1U)) == 0U); }
+
+void *Memory::alloc_aligned_static(size_t p_bytes, size_t p_alignment) {
+	DEV_ASSERT(is_power_of_2(p_alignment));
+
+	void *p1, *p2;
+	if ((p1 = (void *)malloc(p_bytes + p_alignment - 1 + sizeof(uint32_t))) == nullptr) {
+		return nullptr;
+	}
+
+	p2 = (void *)(((uintptr_t)p1 + sizeof(uint32_t) + p_alignment - 1) & ~((p_alignment)-1));
+	*((uint32_t *)p2 - 1) = (uint32_t)((uintptr_t)p2 - (uintptr_t)p1);
+	return p2;
+}
+
+void *Memory::realloc_aligned_static(void *p_memory, size_t p_bytes, size_t p_prev_bytes, size_t p_alignment) {
+	if (p_memory == nullptr) {
+		return alloc_aligned_static(p_bytes, p_alignment);
+	}
+
+	void *ret = alloc_aligned_static(p_bytes, p_alignment);
+	memcpy(ret, p_memory, p_prev_bytes);
+	free_aligned_static(p_memory);
+	return ret;
+}
+
+void Memory::free_aligned_static(void *p_memory) {
+	uint32_t offset = *((uint32_t *)p_memory - 1);
+	void *p = (void *)((uint8_t *)p_memory - offset);
+	free(p);
+}
+
 void *Memory::alloc_static(size_t p_bytes, bool p_pad_align) {
 #ifdef DEBUG_ENABLED
 	bool prepad = true;

core/os/memory.h

@@ -64,6 +64,30 @@ class Memory {
 	static void *realloc_static(void *p_memory, size_t p_bytes, bool p_pad_align = false);
 	static void free_static(void *p_ptr, bool p_pad_align = false);
 
+	//	                            ↓ return value of alloc_aligned_static
+	//	┌─────────────────┬─────────┬─────────┬──────────────────┐
+	//	│ padding (up to  │ uint32_t│ void*   │ padding (up to   │
+	//	│ p_alignment - 1)│ offset  │ p_bytes │ p_alignment - 1) │
+	//	└─────────────────┴─────────┴─────────┴──────────────────┘
+	//
+	// alloc_aligned_static will allocate p_bytes + p_alignment - 1 + sizeof(uint32_t) and
+	// then offset the pointer until alignment is satisfied.
+	//
+	// This offset is stored before the start of the returned ptr so we can retrieve the original/real
+	// start of the ptr in order to free it.
+	//
+	// The rest is wasted as padding in the beginning and end of the ptr. The sum of padding at
+	// both start and end of the block must add exactly to p_alignment - 1.
+	//
+	// p_alignment MUST be a power of 2.
+	static void *alloc_aligned_static(size_t p_bytes, size_t p_alignment);
+	static void *realloc_aligned_static(void *p_memory, size_t p_bytes, size_t p_prev_bytes, size_t p_alignment);
+	// Pass the ptr returned by alloc_aligned_static to free it.
+	// e.g.
+	//	void *data = realloc_aligned_static( bytes, 16 );
+	//  free_aligned_static( data );
+	static void free_aligned_static(void *p_memory);
+
 	static uint64_t get_mem_available();
 	static uint64_t get_mem_usage();
 	static uint64_t get_mem_max_usage();