New heap, simple and bug-free, but inefficient.

6 files changed, 432 insertions, 241 deletions

diff --git a/src/kernel/mem/heap.basic.c b/src/kernel/mem/heap.basic.c
new file mode 100644
index 0000000..d746deb
--- /dev/null
+++ b/src/kernel/mem/heap.basic.c
@@ -0,0 +1,136 @@
+#include "heap.basic.h"
+
+#include "paging.h"
+
+#include <lib/mutex.h>
+#include <core/sys.h>
+
+void heap_create(struct heap *heap, size_t start, size_t datasize, size_t maxdatasize) {
+	uint32_t i;
+	if (start & 0x0FFF) start = (start & 0xFFFFF000) + 0x1000;
+	
+	// Allocate frames
+	for (i = start; i < start + datasize; i += 0x1000) {
+		page_map(pagedir_getPage(kernel_pagedir, i, 1), frame_alloc(), 0, 0);
+	}
+
+	heap->start_addr = start;
+	heap->end_addr = start + datasize;
+	heap->max_end = start + maxdatasize;
+
+	struct heap_header *first = (struct heap_header*)start;
+	first->magic = HH_FREE_MAGIC;
+	first->prev = first->next = first;
+
+	heap->first = first;
+	heap->first_free = first;
+	
+	heap->mutex = 0;
+}
+
+// ***** UTILITY FUNCTIONS *****
+
+#define BLK_SIZE(blk) (blk->next == blk ? ((size_t)heap->end_addr - (size_t)blk) : ((size_t)blk->next - (size_t)blk))
+#define BLK_FREE(blk) (blk->magic == HH_FREE_MAGIC)
+
+void blk_split(struct heap *heap, struct heap_header *hdr, size_t size) {
+	ASSERT (size >= MIN_BLK_SIZE);
+	ASSERT (BLK_SIZE(hdr) > size);
+	ASSERT (BLK_SIZE(hdr) - size >= MIN_BLK_SIZE);
+	ASSERT (BLK_FREE(hdr));
+
+	struct heap_header *hdr_nxt = (struct heap_header*)((size_t)hdr + size);
+	hdr_nxt->magic = HH_FREE_MAGIC;
+	hdr_nxt->prev = hdr;
+	if (hdr->next == hdr) {
+		hdr_nxt->next = hdr_nxt;
+	} else {
+		hdr_nxt->next = hdr->next;
+		hdr->next->prev = hdr_nxt;
+	}
+	hdr->next = hdr_nxt;
+}
+
+void blk_combine_next(struct heap *heap, struct heap_header *hdr) {
+	ASSERT (hdr != hdr->next);	// make sure we are not the last block
+	ASSERT (hdr->magic == HH_FREE_MAGIC);
+	ASSERT (hdr->next->magic == HH_FREE_MAGIC);
+
+	struct heap_header *hdr_nxt = hdr->next;
+	if (hdr_nxt->next == hdr_nxt) {
+		hdr->next = hdr;
+	} else {
+		hdr->next = hdr_nxt->next;
+		hdr_nxt->next->prev = hdr;
+	}
+}
+
+// ***** Allocation/Freeing functions *****
+
+void *heap_alloc(struct heap *heap, size_t sz) {
+	/* Algorithm :
+	1 Use heap->first_free to find first effectively free block, update heap->first_free
+	2 Find first free AND BIG ENOUGH block
+		- If no block can be found, return 0 (no heap expand yet)
+	3 If that block is too big, split it in two
+	4 Mark that block as used and return it's address + sizeof(heap_header) */
+	mutex_lock(&heap->mutex);
+
+	sz += sizeof(struct heap_header);
+	if (sz < MIN_BLK_SIZE) sz = MIN_BLK_SIZE;
+	if (sz % 4 != 0) sz = sz + 4 - (sz % 4); 
+
+	// 1
+	struct heap_header *it = heap->first_free;
+	while (!BLK_FREE(it) && it != it->next) it = it->next;
+	if (!BLK_FREE(it) && it == it->next) {
+		mutex_unlock(&heap->mutex);
+		return 0;
+	}
+	heap->first_free = it;
+
+	// 2
+	while (BLK_SIZE(it) < sz && it != it->next) it = it->next;
+	if (BLK_SIZE(it) < sz) {		// no block could be found
+		mutex_unlock(&heap->mutex);
+		return 0;
+	}
+
+	// 3
+	if (BLK_SIZE(it) >= sz + MIN_BLK_SIZE) blk_split(heap, it, sz);
+
+	// 4
+	void *ret = (void*)((size_t)it + sizeof(struct heap_header));
+	it->magic = HH_ALLOC_MAGIC;
+
+	mutex_unlock(&heap->mutex);
+	return ret;
+}
+
+void heap_free(struct heap *heap, void *ptr) {
+	/* Algorithm :
+	1 Get block pointer to the block we are freeing
+	2 Mark that block as free
+	3 If block->next is free, combine_next(block)
+	4 If block->prev is free, block = block->prev; combine_next(block)
+	5 If block < heap->first_free; heap->first_free = block
+	*/
+	mutex_lock(&heap->mutex);
+
+	// 1
+	struct heap_header *hdr = (struct heap_header*)((size_t)ptr - sizeof(struct heap_header));
+	if (hdr->magic != HH_ALLOC_MAGIC) return;
+	// 2
+	hdr->magic = HH_FREE_MAGIC;
+	// 3
+	if (hdr->next != hdr && hdr->next->magic == HH_FREE_MAGIC) blk_combine_next(heap, hdr);
+	// 4
+	if (hdr->prev != hdr && hdr->prev->magic == HH_FREE_MAGIC) {
+		hdr = hdr->prev;
+		blk_combine_next(heap, hdr);
+	}
+	// 5
+	if (hdr < heap->first_free) heap->first_free = hdr;
+
+	mutex_unlock(&heap->mutex);
+}
diff --git a/src/kernel/mem/heap.basic.h b/src/kernel/mem/heap.basic.h
new file mode 100644
index 0000000..c752750
--- /dev/null
+++ b/src/kernel/mem/heap.basic.h
@@ -0,0 +1,30 @@
+#ifndef DEF_HEAP_H
+#define DEF_HEAP_H
+
+/*	This heap implementation is extra-simple and extra-slow. */
+
+#include "types.h"
+
+#define HH_FREE_MAGIC 0xFEE0FEE0
+#define HH_ALLOC_MAGIC 0xA110A110
+
+#define HEAP_EXTEND_QUANTITY 0x00080000		// each time we extend the heap, extend it 512 k
+
+#define MIN_BLK_SIZE 0x20		// 32 bytes
+
+struct heap_header {
+	uint32_t magic;
+	struct heap_header *prev, *next;
+};
+
+struct heap {
+	struct heap_header *first, *first_free;		// first_free is not necessarily free, it's just an indication
+	size_t start_addr, end_addr, max_end;
+	uint32_t mutex;
+};
+
+void heap_create(struct heap *heap, size_t start, size_t datasize, size_t maxdatasize);
+void *heap_alloc(struct heap *heap, size_t sz);
+void heap_free(struct heap *heap, void *ptr);
+
+#endif
diff --git a/src/kernel/mem/heap.c b/src/kernel/mem/heap.c
index 79ef81e..a590c28 100644
--- a/src/kernel/mem/heap.c
+++ b/src/kernel/mem/heap.c
@@ -1,241 +1,12 @@
-#include "heap.h"
-#include "paging.h"
-#include <core/sys.h>
+// THIS EITHER INCLUDES heap.std.c OR heap.basic.c DEPENDING ON
+// WHAT'S SPECIFIED IN config.h
 
-#include <lib/stdlib.h>
+#include <config.h>
 
-#define HEAP_MAGIC 0xBAD0BEEF
-#define HEAP_MIN_SIZE 0x4000
+#ifdef K_USE_BASIC_HEAP
+#include "heap.basic.c"
 
-/* *******************			HEADER    	   ****************** */
+#else
+#include "heap.std.c"
 
-/*	For internal use only. Inserts a hole in the heap's hole index at the correct position. */
-static void heapIdx_insert(struct heap *heap, struct heap_header *e) {
-	if ((heap->idxused + sizeof(struct heap_header*) + (size_t)heap->idx) >= heap->start_addr) return;
-
-	uint32_t iterator = 0, pos;
-	while (iterator < heap->idxused && heap->idx[iterator]->size <= e->size) {
-		if (heap->idx[iterator] == e) return;
-		iterator++;
-	}
-	if (iterator == heap->idxused) {
-		heap->idx[heap->idxused++] = e;
-	} else {
-		pos = iterator;
-		iterator = heap->idxused;
-		while (iterator > pos) {
-			heap->idx[iterator] = heap->idx[iterator - 1];
-			iterator--;
-		}
-		heap->idxused++;
-		heap->idx[pos] = e;
-	}
-}
-
-/* For internal use only. Removes a hole from the heap's hole index. */
-static void heapIdx_remove(struct heap *heap, struct heap_header *e) {
-	uint32_t iterator;
-	for (iterator = 0; iterator < heap->idxused; iterator++) {
-		if (heap->idx[iterator] == e) break;
-	}
-	if (iterator == heap->idxused) return;
-	heap->idxused--;
-	while (iterator < heap->idxused) {
-		heap->idx[iterator] = heap->idx[iterator + 1];
-		iterator++;
-	}
-}
-
-/* ********************			CONTENTS		********************* */
-
-/*	Initializes the heap, creates the correct data structures. */
-void heap_create(struct heap *heap, size_t start, size_t idxsize, size_t datasize, size_t maxdatasize) {
-	uint32_t i;
-
-	if (start & 0x0FFF) start = (start & 0xFFFFF000) + 0x1000;
-
-	heap->start_addr = start + idxsize;
-	heap->end_addr = start + idxsize + datasize;
-	heap->max_end = start + idxsize + maxdatasize;
-
-	for (i = start; i < heap->end_addr; i += 0x1000) {
-		page_map(pagedir_getPage(kernel_pagedir, i, 1), frame_alloc(), 0, 0);
-	}
-
-	heap->idx = (struct heap_header**)start;	
-	heap->idxused = 0;
-
-	struct heap_header *hole = (struct heap_header*) heap->start_addr;
-	hole->size = (heap->end_addr - heap->start_addr);
-	hole->magic = HEAP_MAGIC;
-	hole->is_hole = 1;
-
-	struct heap_footer *hole_footer = (struct heap_footer*)(heap->end_addr - sizeof(struct heap_footer));
-	hole_footer->header = hole;
-	hole_footer->magic = HEAP_MAGIC;
-
-	heapIdx_insert(heap, hole);
-}
-
-/*	For internal use only. Called by heap_alloc when necessary. Expands the heap to take more space. */
-static uint32_t heap_expand(struct heap *heap, size_t quantity) {
-	uint32_t i;
-
-	if (quantity & 0x0FFF) {
-		quantity = (quantity & 0xFFFFF000) + 0x1000;
-	}
-
-	if (heap->end_addr + quantity > heap->max_end) return 0;
-
-	size_t newEnd = heap->end_addr + quantity;
-
-	for (i = heap->end_addr; i < newEnd; i += 0x1000) {
-		page_map(pagedir_getPage(kernel_pagedir, i, 1), frame_alloc(), 0, 0);
-	}
-
-	struct heap_footer *last_footer = (struct heap_footer*)(heap->end_addr - sizeof(struct heap_footer));
-	struct heap_header *last_header = last_footer->header;
-	if (last_header->is_hole) {
-		heapIdx_remove(heap, last_header);
-		last_header->size += quantity;
-
-		last_footer = (struct heap_footer*)(newEnd - sizeof(struct heap_footer));
-		last_footer->magic = HEAP_MAGIC;
-		last_footer->header = last_header;
-
-		heapIdx_insert(heap, last_header);
-	} else {
-		last_header = (struct heap_header*)heap->end_addr;
-		last_footer = (struct heap_footer*)(newEnd - sizeof(struct heap_footer));
-
-		last_header->is_hole = 1;
-		last_header->magic = HEAP_MAGIC;
-		last_header->size = quantity;
-
-		last_footer->magic = HEAP_MAGIC;
-		last_footer->header = last_header;
-
-		heapIdx_insert(heap, last_header);
-	}
-
-	heap->end_addr = newEnd;
-
-	return 1;
-}
-
-/*	For internal use only. Called by heap_free when necessary. Reduces the heap's size. */
-static void heap_contract(struct heap *heap) {
-	return;		//TODO: this function bugs everything
-
-
-	struct heap_footer *last_footer = (struct heap_footer*)(heap->end_addr - sizeof(struct heap_footer));
-	struct heap_header *last_header = last_footer->header;
-
-	if (last_header->is_hole == 0) return;
-	if (last_header->size <= 0x1000) return;
-
-	size_t quantity = ((heap->end_addr - heap->start_addr) & 0xFFFFF000) - 0x1000;
-	while ((heap->end_addr - heap->start_addr) - quantity < HEAP_MIN_SIZE ||
-			last_header->size - 0x4000 < quantity)
-		quantity -= 0x1000;
-	if (quantity == 0) return;
-
-	size_t newEnd = heap->end_addr - quantity;
-
-	heapIdx_remove(heap, last_header);
-	last_header->size -= quantity;
-	last_footer = (struct heap_footer*)((size_t)last_footer - quantity);
-	last_footer->magic = HEAP_MAGIC;
-	last_footer->header = last_header;
-	heapIdx_insert(heap, last_header);
-
-	for (heap->end_addr -= 0x1000; heap->end_addr >= newEnd; heap->end_addr -= 0x1000) {
-		page_unmapFree(pagedir_getPage(kernel_pagedir, heap->end_addr, 0));
-	}
-}
-
-/*	Alocate some bytes on the heap. */
-void* heap_alloc(struct heap *heap, size_t sz) {
-	ASSERT(heap > 0xE0000000);
-	size_t newsize = sz + sizeof(struct heap_header) + sizeof(struct heap_footer);
-	uint32_t iterator = 0;
-
-	while (iterator < heap->idxused) {
-		if (heap->idx[iterator]->size >= newsize) break;
-		iterator++;
-	}
-
-	if (iterator == heap->idxused) {	//No hole is big enough
-		if (heap_expand(heap,	
-			MAX((heap->end_addr - heap->start_addr) & 0xFFFFF000, (newsize & 0xFFFFF000) + 0x1000)
-			) == 0) return 0;	//FAILED
-		return heap_alloc(heap, sz);
-	}
-
-	struct heap_header *loc = heap->idx[iterator];
-	heapIdx_remove(heap, loc);
-	struct heap_footer *footer = (struct heap_footer*)((size_t)loc + loc->size - sizeof(struct heap_footer));
-	loc->is_hole = 0;
-	
-	//If we have enough space to create a USEFUL new hole next to the allocated block, do it.
-	//If we do not, we might return a block that is a few bytes bigger than needed.
-	if (loc->size > (newsize + sizeof(struct heap_header) + sizeof(struct heap_footer))) {
-		loc->size = newsize;
-
-		//Write footer for block we return
-		struct heap_footer *newfooter = (struct heap_footer*)((size_t)loc + newsize - sizeof(struct heap_footer));
-		newfooter->header = loc;
-		newfooter->magic = HEAP_MAGIC;
-
-		//Write header for new hole we create
-		struct heap_header *nextloc = (struct heap_header*)((size_t)loc + newsize);
-		nextloc->is_hole = 1;
-		nextloc->magic = HEAP_MAGIC;
-		nextloc->size = ((size_t)footer - (size_t)nextloc + sizeof(struct heap_footer));
-		footer->header = nextloc;	//Update footer
-		footer->magic = HEAP_MAGIC;
-
-		heapIdx_insert(heap, nextloc);
-	}
-
-	return (void*)((size_t)loc + sizeof(struct heap_header));
-}
-
-/*	Frees a block previously allocated on the heap. */
-void heap_free(struct heap *heap, void* ptr) {
-	if (ptr == 0) return;
-	if ((size_t)ptr < heap->start_addr || (size_t)ptr > heap->end_addr) return;
-
-	struct heap_header *header = (struct heap_header*)((size_t)ptr - sizeof(struct heap_header));
-	struct heap_footer *footer = (struct heap_footer*)((size_t)header + header->size - sizeof(struct heap_footer));
-	if (header->magic != HEAP_MAGIC || footer->magic != HEAP_MAGIC) return;
-
-	//Unify left
-	struct heap_footer *prev_footer = (struct heap_footer*)((size_t)header - sizeof(struct heap_footer));
-	if (prev_footer->magic == HEAP_MAGIC && prev_footer->header->is_hole) {
-		header = prev_footer->header;
-		heapIdx_remove(heap, header);
-
-		footer->header = header;
-		header->size = ((size_t)footer - (size_t)header + sizeof(struct heap_footer));
-	}
-
-	//Unify right
-	struct heap_header *next_header = (struct heap_header*)((size_t)footer + sizeof(struct heap_footer));
-	if (next_header->magic == HEAP_MAGIC && next_header->is_hole) {
-		heapIdx_remove(heap, next_header);
-		footer = (struct heap_footer*)((size_t)footer + next_header->size);
-
-		footer->header = header;
-		header->size = ((size_t)footer - (size_t)header + sizeof(struct heap_footer));
-	}
-
-	header->is_hole = 1;
-
-	heapIdx_insert(heap, header);
-
-	if ((size_t)footer == (heap->end_addr - sizeof(struct heap_footer)) &&
-			header->size >= 0x2000 && (heap->end_addr - heap->start_addr > HEAP_MIN_SIZE)) {
-		heap_contract(heap);
-	}	
-}
+#endif
diff --git a/src/kernel/mem/heap.std.c b/src/kernel/mem/heap.std.c
new file mode 100644
index 0000000..5b2d025
--- /dev/null
+++ b/src/kernel/mem/heap.std.c
@@ -0,0 +1,241 @@
+#include "heap.std.h"
+#include "paging.h"
+#include <core/sys.h>
+
+#include <lib/stdlib.h>
+
+#define HEAP_MAGIC 0xBAD0BEEF
+#define HEAP_MIN_SIZE 0x4000
+
+/* *******************			HEADER    	   ****************** */
+
+/*	For internal use only. Inserts a hole in the heap's hole index at the correct position. */
+static void heapIdx_insert(struct heap *heap, struct heap_header *e) {
+	if ((heap->idxused + sizeof(struct heap_header*) + (size_t)heap->idx) >= heap->start_addr) return;
+
+	uint32_t iterator = 0, pos;
+	while (iterator < heap->idxused && heap->idx[iterator]->size <= e->size) {
+		if (heap->idx[iterator] == e) return;
+		iterator++;
+	}
+	if (iterator == heap->idxused) {
+		heap->idx[heap->idxused++] = e;
+	} else {
+		pos = iterator;
+		iterator = heap->idxused;
+		while (iterator > pos) {
+			heap->idx[iterator] = heap->idx[iterator - 1];
+			iterator--;
+		}
+		heap->idxused++;
+		heap->idx[pos] = e;
+	}
+}
+
+/* For internal use only. Removes a hole from the heap's hole index. */
+static void heapIdx_remove(struct heap *heap, struct heap_header *e) {
+	uint32_t iterator;
+	for (iterator = 0; iterator < heap->idxused; iterator++) {
+		if (heap->idx[iterator] == e) break;
+	}
+	if (iterator == heap->idxused) return;
+	heap->idxused--;
+	while (iterator < heap->idxused) {
+		heap->idx[iterator] = heap->idx[iterator + 1];
+		iterator++;
+	}
+}
+
+/* ********************			CONTENTS		********************* */
+
+/*	Initializes the heap, creates the correct data structures. */
+void heap_create(struct heap *heap, size_t start, size_t idxsize, size_t datasize, size_t maxdatasize) {
+	uint32_t i;
+
+	if (start & 0x0FFF) start = (start & 0xFFFFF000) + 0x1000;
+
+	heap->start_addr = start + idxsize;
+	heap->end_addr = start + idxsize + datasize;
+	heap->max_end = start + idxsize + maxdatasize;
+
+	for (i = start; i < heap->end_addr; i += 0x1000) {
+		page_map(pagedir_getPage(kernel_pagedir, i, 1), frame_alloc(), 0, 0);
+	}
+
+	heap->idx = (struct heap_header**)start;	
+	heap->idxused = 0;
+
+	struct heap_header *hole = (struct heap_header*) heap->start_addr;
+	hole->size = (heap->end_addr - heap->start_addr);
+	hole->magic = HEAP_MAGIC;
+	hole->is_hole = 1;
+
+	struct heap_footer *hole_footer = (struct heap_footer*)(heap->end_addr - sizeof(struct heap_footer));
+	hole_footer->header = hole;
+	hole_footer->magic = HEAP_MAGIC;
+
+	heapIdx_insert(heap, hole);
+}
+
+/*	For internal use only. Called by heap_alloc when necessary. Expands the heap to take more space. */
+static uint32_t heap_expand(struct heap *heap, size_t quantity) {
+	uint32_t i;
+
+	if (quantity & 0x0FFF) {
+		quantity = (quantity & 0xFFFFF000) + 0x1000;
+	}
+
+	if (heap->end_addr + quantity > heap->max_end) return 0;
+
+	size_t newEnd = heap->end_addr + quantity;
+
+	for (i = heap->end_addr; i < newEnd; i += 0x1000) {
+		page_map(pagedir_getPage(kernel_pagedir, i, 1), frame_alloc(), 0, 0);
+	}
+
+	struct heap_footer *last_footer = (struct heap_footer*)(heap->end_addr - sizeof(struct heap_footer));
+	struct heap_header *last_header = last_footer->header;
+	if (last_header->is_hole) {
+		heapIdx_remove(heap, last_header);
+		last_header->size += quantity;
+
+		last_footer = (struct heap_footer*)(newEnd - sizeof(struct heap_footer));
+		last_footer->magic = HEAP_MAGIC;
+		last_footer->header = last_header;
+
+		heapIdx_insert(heap, last_header);
+	} else {
+		last_header = (struct heap_header*)heap->end_addr;
+		last_footer = (struct heap_footer*)(newEnd - sizeof(struct heap_footer));
+
+		last_header->is_hole = 1;
+		last_header->magic = HEAP_MAGIC;
+		last_header->size = quantity;
+
+		last_footer->magic = HEAP_MAGIC;
+		last_footer->header = last_header;
+
+		heapIdx_insert(heap, last_header);
+	}
+
+	heap->end_addr = newEnd;
+
+	return 1;
+}
+
+/*	For internal use only. Called by heap_free when necessary. Reduces the heap's size. */
+static void heap_contract(struct heap *heap) {
+	return;		//TODO: this function bugs everything
+
+
+	struct heap_footer *last_footer = (struct heap_footer*)(heap->end_addr - sizeof(struct heap_footer));
+	struct heap_header *last_header = last_footer->header;
+
+	if (last_header->is_hole == 0) return;
+	if (last_header->size <= 0x1000) return;
+
+	size_t quantity = ((heap->end_addr - heap->start_addr) & 0xFFFFF000) - 0x1000;
+	while ((heap->end_addr - heap->start_addr) - quantity < HEAP_MIN_SIZE ||
+			last_header->size - 0x4000 < quantity)
+		quantity -= 0x1000;
+	if (quantity == 0) return;
+
+	size_t newEnd = heap->end_addr - quantity;
+
+	heapIdx_remove(heap, last_header);
+	last_header->size -= quantity;
+	last_footer = (struct heap_footer*)((size_t)last_footer - quantity);
+	last_footer->magic = HEAP_MAGIC;
+	last_footer->header = last_header;
+	heapIdx_insert(heap, last_header);
+
+	for (heap->end_addr -= 0x1000; heap->end_addr >= newEnd; heap->end_addr -= 0x1000) {
+		page_unmapFree(pagedir_getPage(kernel_pagedir, heap->end_addr, 0));
+	}
+}
+
+/*	Alocate some bytes on the heap. */
+void* heap_alloc(struct heap *heap, size_t sz) {
+	ASSERT(heap > 0xE0000000);
+	size_t newsize = sz + sizeof(struct heap_header) + sizeof(struct heap_footer);
+	uint32_t iterator = 0;
+
+	while (iterator < heap->idxused) {
+		if (heap->idx[iterator]->size >= newsize) break;
+		iterator++;
+	}
+
+	if (iterator == heap->idxused) {	//No hole is big enough
+		if (heap_expand(heap,	
+			MAX((heap->end_addr - heap->start_addr) & 0xFFFFF000, (newsize & 0xFFFFF000) + 0x1000)
+			) == 0) return 0;	//FAILED
+		return heap_alloc(heap, sz);
+	}
+
+	struct heap_header *loc = heap->idx[iterator];
+	heapIdx_remove(heap, loc);
+	struct heap_footer *footer = (struct heap_footer*)((size_t)loc + loc->size - sizeof(struct heap_footer));
+	loc->is_hole = 0;
+	
+	//If we have enough space to create a USEFUL new hole next to the allocated block, do it.
+	//If we do not, we might return a block that is a few bytes bigger than needed.
+	if (loc->size > (newsize + sizeof(struct heap_header) + sizeof(struct heap_footer))) {
+		loc->size = newsize;
+
+		//Write footer for block we return
+		struct heap_footer *newfooter = (struct heap_footer*)((size_t)loc + newsize - sizeof(struct heap_footer));
+		newfooter->header = loc;
+		newfooter->magic = HEAP_MAGIC;
+
+		//Write header for new hole we create
+		struct heap_header *nextloc = (struct heap_header*)((size_t)loc + newsize);
+		nextloc->is_hole = 1;
+		nextloc->magic = HEAP_MAGIC;
+		nextloc->size = ((size_t)footer - (size_t)nextloc + sizeof(struct heap_footer));
+		footer->header = nextloc;	//Update footer
+		footer->magic = HEAP_MAGIC;
+
+		heapIdx_insert(heap, nextloc);
+	}
+
+	return (void*)((size_t)loc + sizeof(struct heap_header));
+}
+
+/*	Frees a block previously allocated on the heap. */
+void heap_free(struct heap *heap, void* ptr) {
+	if (ptr == 0) return;
+	if ((size_t)ptr < heap->start_addr || (size_t)ptr > heap->end_addr) return;
+
+	struct heap_header *header = (struct heap_header*)((size_t)ptr - sizeof(struct heap_header));
+	struct heap_footer *footer = (struct heap_footer*)((size_t)header + header->size - sizeof(struct heap_footer));
+	if (header->magic != HEAP_MAGIC || footer->magic != HEAP_MAGIC) return;
+
+	//Unify left
+	struct heap_footer *prev_footer = (struct heap_footer*)((size_t)header - sizeof(struct heap_footer));
+	if (prev_footer->magic == HEAP_MAGIC && prev_footer->header->is_hole) {
+		header = prev_footer->header;
+		heapIdx_remove(heap, header);
+
+		footer->header = header;
+		header->size = ((size_t)footer - (size_t)header + sizeof(struct heap_footer));
+	}
+
+	//Unify right
+	struct heap_header *next_header = (struct heap_header*)((size_t)footer + sizeof(struct heap_footer));
+	if (next_header->magic == HEAP_MAGIC && next_header->is_hole) {
+		heapIdx_remove(heap, next_header);
+		footer = (struct heap_footer*)((size_t)footer + next_header->size);
+
+		footer->header = header;
+		header->size = ((size_t)footer - (size_t)header + sizeof(struct heap_footer));
+	}
+
+	header->is_hole = 1;
+
+	heapIdx_insert(heap, header);
+
+	if ((size_t)footer == (heap->end_addr - sizeof(struct heap_footer)) &&
+			header->size >= 0x2000 && (heap->end_addr - heap->start_addr > HEAP_MIN_SIZE)) {
+		heap_contract(heap);
+	}	
+}
diff --git a/src/kernel/mem/heap.h b/src/kernel/mem/heap.std.h
index 8f8cdcf..a7ba173 100644
--- a/src/kernel/mem/heap.h
+++ b/src/kernel/mem/heap.std.h
@@ -1,9 +1,11 @@
 #ifndef DEF_HEAP_H
 #define DEF_HEAP_H
 
+#error "Using standard heap ! It's all buggy everywhere !"
+
 /*	The heap is the data structure that permits allocating and freeing memory easily.
 	The functions in this file are only used by mem.c, which provides kmalloc and kfree.
-	The heap algorithm used is the one described here : 
+	The heap algorithm used is based on the one described here : 
 	http://www.jamesmolloy.co.uk/tutorial_html/7.-The%20Heap.html */
 
 #include "types.h"
diff --git a/src/kernel/mem/mem.c b/src/kernel/mem/mem.c
index 2061aff..c6494b7 100644
--- a/src/kernel/mem/mem.c
+++ b/src/kernel/mem/mem.c
@@ -2,12 +2,19 @@
 #include <core/sys.h>
 #include <core/monitor.h>
 #include "paging.h"
-#include "heap.h"
+
+#include <config.h>
+
+#ifdef K_USE_BASIC_HEAP
+#include "heap.basic.h"
+#else
+#include "heap.std.h"
+#endif
 
 #define FREEPAGESTOKEEP 5
 
-#define KHEAP_IDXSIZE 0x1000
-#define KHEAP_INITSIZE 0x80000
+#define KHEAP_IDXSIZE 0x4000		// only used with heap.std.h
+#define KHEAP_INITSIZE 0x00080000
 #define KHEAP_MAXSIZE 0x08000000
 
 size_t mem_placementAddr;
@@ -76,7 +83,11 @@ static struct heap kheap;
 
 /*	Called on kernel start. Creates the kernel heap. */
 void kheap_init() {
+#ifdef K_USE_BASIC_HEAP
+	heap_create(&kheap, (mem_placementAddr & 0xFFFFF000) + 0x1000, KHEAP_INITSIZE, KHEAP_MAXSIZE);
+#else
 	heap_create(&kheap, (mem_placementAddr & 0xFFFFF000) + 0x1000, KHEAP_IDXSIZE, KHEAP_INITSIZE, KHEAP_MAXSIZE);
+#endif
 	kheap_working = 1;
 	monitor_write("[KHeap] ");
 }