From 13be2de4b4cae3cfd03df73697238e03643436a6 Mon Sep 17 00:00:00 2001
From: Ralph Ronnquist <ralph.ronnquist@gmail.com>
Date: Sat, 23 Jul 2022 08:40:47 +1000
Subject: [PATCH] memory management for efi

---
 efimm/Heap.c | 134 +++++++++++++++++++++++++++++++++++++++++++++++++++
 efimm/Heap.h |  14 ++++++
 2 files changed, 148 insertions(+)
 create mode 100644 efimm/Heap.c
 create mode 100644 efimm/Heap.h

diff --git a/efimm/Heap.c b/efimm/Heap.c
new file mode 100644
index 0000000..74467e8
--- /dev/null
+++ b/efimm/Heap.c
@@ -0,0 +1,134 @@
+#include <efi.h>
+#include <efiapi.h>
+#include <HashVector.h>
+
+#define LARGE_HEAP_SIZE (1048576)
+
+#define CEILX(V,D) ((int)(((V)+(D)-1)/(D)))
+#define CEIL(V,D) (CEILX(V,D)*(D))
+
+extern EFI_BOOT_SERVICES *boot_services;
+
+static void *real_malloc(size_t size) {
+    size_t pages = CEILX( size, EFI_PAGE_SIZE );
+    EFI_PHYSICAL_ADDRESS p = 0;
+    if ( boot_services->AllocatePages(
+	     AllocateAnyPages, EfiLoaderData, pages, &p ) != EFI_SUCCESS ) {
+	return 0;
+    }
+    return (void*) p;
+}
+
+static void real_free(void *p,size_t size) {
+    size_t pages = CEILX( size, EFI_PAGE_SIZE );
+    (void) boot_services->FreePages( (EFI_PHYSICAL_ADDRESS) p, pages );
+}
+
+static void memclear(char *p,size_t size) {
+    char *end = p + size;
+    while ( p < end ) {
+	*(p++) = 0;
+    }
+}
+
+typedef struct ChunkHead {
+    size_t size;
+    struct ChunkHead *data;
+} ChunkHead;
+
+static ChunkHead *free_chunks;
+
+void* malloc(size_t size) {
+    // Adjust allocation size to include a preceding size count
+    size_t asize = CEIL( size + sizeof( size_t ), sizeof( ChunkHead ) );
+    if ( asize >= LARGE_HEAP_SIZE ) {
+	// Managed without subdivision
+	ChunkHead *p = real_malloc( asize );
+	p->size = asize;
+	return (void*) &p->data;
+    }
+    // Cut out a chunk by subdivision adjusted to an even number of
+    // ChunkHead;
+    ChunkHead *this = free_chunks;
+    ChunkHead *prev = 0;
+    for ( ; this ; prev = this, this = this->data ) {
+	if ( this->size < asize ) {
+	    continue;
+	}
+	// Cut out an allocation chunk here,
+	size_t rest = this->size - asize;
+	ChunkHead *next = this->data;
+	if ( rest >= sizeof( ChunkHead ) ) {
+	    next = (ChunkHead *) (((char*)this) + asize);
+	    next->data = this->data;
+	    next->size = rest;
+	}
+	if ( prev ) {
+	    prev->data = next;
+	} else {
+	    free_chunks = next;
+	}
+	memclear( (char*) this, sizeof( ChunkHead ) );
+	this->size = asize;
+	return (void*) ((char*)this) + sizeof( size_t );
+    }
+    // No subdivision available; add a new subdivision page and try
+    // again. Note that this contains its own ChunkHead.
+    this = (ChunkHead *) real_malloc( LARGE_HEAP_SIZE );
+    memclear( (char*) this, LARGE_HEAP_SIZE );
+    this->size = LARGE_HEAP_SIZE;
+    this->data = free_chunks;
+    free_chunks = this;
+    return malloc( size );
+}
+
+void free(void *p) {
+    if ( p == 0 ) {
+	return;
+    }
+    ChunkHead *chp = (ChunkHead *) ( ((char*) p) - sizeof( size_t ) );
+    if ( chp->size >= LARGE_HEAP_SIZE ) {
+	real_free( p, chp->size );
+	return;
+    } 
+    // find nearest ChunkHead in the free_chunks list.
+    ChunkHead *this = free_chunks;
+    ChunkHead *prev = 0;
+    for ( ; this; prev = this, this = this->data ) {
+	if ( (void*) this >= p ) {
+	    break;
+	}
+    }
+    this = chp;
+    if ( prev == 0 ) {
+	this->data = free_chunks;
+	free_chunks = this;
+    } else {
+	this->data = prev->data;
+    }
+    if ( ((char*)prev->data) + prev->size == (char*)this ) {
+	// collapse into single area
+	prev->size += this->size;
+	this = prev;
+    }
+    // Maybe this emptied a subdivision block, except the last?
+    if ( this->data && ((char*)this) + this->size == (char*)(this->data) ) {
+	this->size += this->data->size;
+    }
+    // Yes. That should also be freed
+    if ( this->size == LARGE_HEAP_SIZE || this->data ) {
+	this = free_chunks;
+	prev = 0;
+	for ( ; this; prev = this, this = this->data ) {
+	    if ( this == chp ) {
+		if ( prev ) {
+		    prev->data = this->data;
+		} else {
+		    free_chunks = this->data;
+		}
+		real_free( this, this->size );
+		return;
+	    }
+        }
+    }
+}
diff --git a/efimm/Heap.h b/efimm/Heap.h
new file mode 100644
index 0000000..76396fc
--- /dev/null
+++ b/efimm/Heap.h
@@ -0,0 +1,14 @@
+#ifndef Heap_H
+#define Head_H
+
+/**
+ * \brief Allocate a memory block of at least the given size (in bytes).
+ */
+void* malloc(size_t size);
+
+/**
+ * \brief Reclaim allocated memory returned from \ref malloc.
+ */
+void free(void *p);
+
+#endif
-- 
2.39.2