--- a/cos/kernel/kernel.c	Wed Nov 30 22:41:51 2011 +0100
+++ b/cos/kernel/kernel.c	Thu Dec 01 21:42:59 2011 +0100
@@ -126,34 +126,93 @@
   buffer[i] = 0;
 }
 
+#define HEAP_MAGIC 0xc0ffee
+#define HEAP_START 0x400000
+#define HEAP_SIZE  0x200000
+#define HEAP_INUSE 1
+#define HEAP_FREE 0
+
+typedef struct {
+   uint64_t magic;
+   uint64_t state;
+   uint64_t size;
+} heap_t;
+
 /*
   malloc and free divide the chunks of memory present at the heap
   of the kernel into smaller parts.
   The heap is located at: 0x
 */
-static void* kernel_heap = (void*) 0x400000; // 4 MB - 6 MB is heap
+static heap_t* kernel_heap = (heap_t*) 0x400000; // 4 MB - 6 MB is heap
 /* Allocates 'size' bytes and returns the pointer if succesfull.
    Kernelpanic in case of failure..
 */
+
 void* kmalloc(uint64_t size) {
-  printf("Malloc %d bytes\n", size);
-  return kernel_heap;
+   // printf("Malloc %d bytes\n", size);
+
+   // Start at the beginning of our heap and search a free block:
+   heap_t *current = kernel_heap;
+   while (current->magic == HEAP_MAGIC)
+   {
+      if ((current->state == HEAP_FREE) && (current->size >= size))
+      {
+         // Mark block as used:
+         current->state = HEAP_INUSE;
+
+         // Insert a heap header if required:
+         if (current->size > size + sizeof(heap_t) + 1)
+         {
+            // Calculate location of the inserted header:
+            heap_t *newheader = (heap_t*) (((char*)current)+size+sizeof(heap_t));
+
+            // Set the new header fields:
+            newheader->size = current->size - size - sizeof(heap_t);
+            newheader->state = HEAP_FREE;
+            newheader->magic = HEAP_MAGIC;
+
+            // Set the size of this block
+            current->size = size; 
+         }
+         else
+         {
+            // We allocate this whole block
+         }
+         // Calculate the size of the block:
+         char *address = ((char*)current)+sizeof(heap_t);
+         return address;
+         
+      }
+      // Goto next heap block:
+      current = (heap_t*)(((char*) current) + current->size + sizeof(heap_t));
+   }
+   return 0x0;
 }
 
 void kfree(void* ptr) {
   printf("Free address %x\n", ptr);
 }
 
+void init_heap(void)
+{
+   // Initialize the kernel heap:
+   kernel_heap->magic = HEAP_MAGIC;
+   kernel_heap->state = HEAP_FREE;
+   kernel_heap->size = HEAP_SIZE - sizeof(heap_t);
+}
+
 void startPython()
 {
    // TODO: connect to Py_Main
-   PyRun_SimpleString("print('hello world')");
+   //PyRun_SimpleString("print('hello world')");
 
 }
 
 void testMalloc()
 {
    char *a, *b;
+
+   printf("Testing malloc\n");
    a = kmalloc(100);
    printf("Got a at %x\n", a);
    a[0] = 'A';
@@ -161,18 +220,15 @@
    printf("Got b at %x\n", b);
    b[0] = 'B';
    kfree(a);
-
 }
 
 void kmain()
 {
   init_screen();
   setupIDT();
+  init_heap();
 
-  printf("Welcome! .. ");
-  printf("Testing malloc");
-  testMalloc();
-  printf("Entering mainloop!\n");
+  printf("Welcome!\n");
 
   while (1==1) 
   {
@@ -186,6 +242,10 @@
     {
       printf("System time in ms: %d\n", getTimeMS());
     }
+    if (buffer[0] == 't')
+    {
+        testMalloc();
+    }
     if ( strncmp(buffer, "help", 4)) 
     {
       printf("Help\n Try one of these commands:\n");