diff --git a/src/kernel/kheap.c b/src/kernel/kheap.c
new file mode 100644
index 0000000..31de626
--- /dev/null
+++ b/src/kernel/kheap.c
@@ -0,0 +1,138 @@
+#include "kheap.h"
+#include "log.h"
+
+static void swap_entries(struct heap_entry *a, struct heap_entry *b)
+{
+	// Static to not waste stack space, NOT thread safe
+	static struct heap_entry tmp;
+	tmp = *a;
+	*a = *b;
+	*b = tmp;
+}
+
+int heap_parent(int i)
+{
+	return (i - 1) / 2;
+}
+
+int heap_left(int i)
+{
+	return 2 * i + 1;
+}
+
+int heap_right(int i)
+{
+	return 2 * i + 2;
+}
+
+static void heap_sort(struct min_heap *heap, int i)
+{
+	int l = heap_left(i), r = heap_right(i);
+	int smallest = i;
+	int size = heap->size;
+	struct heap_entry *e = heap->elements;
+
+	if (l < size && e[l].key < e[smallest].key)
+		smallest = l;
+	if (r < size && e[r].key < e[smallest].key)
+		smallest = r;
+
+	if (smallest != i)
+	{
+		swap_entries(&e[i], &e[smallest]);
+		heap_sort(heap, smallest);
+	}
+}
+
+static void heap_delete_root(struct min_heap *heap)
+{
+	if (heap->size <= 0)
+		return;
+	if (heap->size == 1)
+		heap->size--;
+
+	heap->elements[0] = heap->elements[--heap->size];
+	heap_sort(heap, 0);
+}
+
+void heap_decrease(struct min_heap *heap, int i, struct heap_entry to)
+{
+	heap->elements[i] = to;
+	int k = to.key;
+
+	while (i && heap->elements[heap_parent(i)].key > k)
+	{
+		swap_entries(&heap->elements[i], &heap->elements[heap_parent(i)]);
+		i = heap_parent(i);
+	}
+}
+
+void heap_delete(struct min_heap *heap, int i)
+{
+	heap_decrease(heap, i,
+				  (struct heap_entry){
+					  .key = -1,
+					  .address = 0,
+				  });
+	heap_delete_root(heap);
+}
+
+void heap_insert(struct min_heap *heap, struct heap_entry e)
+{
+	kassert(heap->size < HEAP_SIZE, "Heap overflow!");
+
+	int i = heap->size++;
+	uint k = e.key;
+	heap->elements[i] = e;
+
+	// While the parent element is GREATER than the child
+	while (i && heap->elements[heap_parent(i)].key > k)
+	{
+		swap_entries(&heap->elements[i], &heap->elements[heap_parent(i)]);
+		i = heap_parent(i);
+	}
+}
+
+struct heap_entry heap_lookup_min(struct min_heap *heap, int min, bool *ok,
+								  bool delete, int *i)
+{
+	int j = 0;
+	struct heap_entry *e = heap->elements;
+
+	while (j < heap->size && e[j].key < min)
+	{
+		int left = heap_left(j), right = heap_right(j);
+
+		// If left is invalid, use right, if right is invalid, use left.
+		// If both are valid, chose the smallest one.
+		// In the first case, right may too be invalid, but it will be
+		// sorted out in the while loop (see else clause at end of function).
+
+		if (left >= heap->size)
+			j = right;
+		else if (right >= heap->size)
+			j = left;
+		else
+			j = e[left].key < e[right].key ? left : right;
+	}
+
+	if (j < heap->size)
+	{
+		struct heap_entry val = e[j];
+
+		*ok = true;
+
+		if (i)
+			*i = j;
+
+		if (delete)
+			heap_delete(heap, j);
+
+		return val;
+	}
+	else
+	{
+		*ok = false;
+		return (struct heap_entry){0};
+	}
+}