diff --git a/kernel/drivers/kheap.c b/kernel/drivers/kheap.c
index 93ad0da..9f46758 100644
--- a/kernel/drivers/kheap.c
+++ b/kernel/drivers/kheap.c
@@ -2,10 +2,52 @@
 
 // Extern variables are declared in the linker script
 extern uint32_t end;
-uint32_t placement_addr = (uint32_t)&end;
 extern page_directory_t *kernel_directory; // FIXME:
+uint32_t placement_addr = (uint32_t)&end;
 heap_t *kheap = 0;
 
+uint32_t kmalloc_init(uint32_t sz, int32_t align, uint32_t *phys) {
+    if(kheap != 0) {
+        void *addr = alloc(sz, (uint8_t)align, &kheap);
+        if(phys != 0) {
+            page_t *page = get_page((uint32_t)addr, 0, kernel_directory);
+            *phys = page->frame*0x1000 + (uint32_t)addr&0xFFF;
+        }
+        return (uint32_t)addr;
+    } else {
+        if(align == 1 && (placement_addr & 0xFFFFF000)) {
+            // Align
+            placement_addr &= 0xFFFFF000;
+            placement_addr += 0x1000;
+        } 
+        if(phys)
+            *phys = placement_addr;
+        uint32_t tmp = placement_addr;
+        placement_addr += sz;
+        return tmp;
+    }
+}
+
+void kfree(void *p) {
+    free(p, kheap);
+}
+
+uint32_t kmalloc_a(uint32_t sz) {
+    return kmalloc_int(sz, 1, 0);
+}
+
+uint32_t kmalloc_p(uint32_t sz, uint32_t *phys) {
+    return kmalloc_int(sz, 0, phys);
+}
+
+uint32_t kmalloc_ap(uint32_t sz, uint32_t *phys) {
+    return kmalloc_int(sz, 1, phys);
+}
+
+uint32_t kmalloc(uint32_t sz) {
+    return kmalloc_int(sz, 0, 0);
+}
+
 /* The following function is a simple method to find the smallest hole that
  * fit user space request, since we will do this process many times, it's a good
  * idea to wrap it in a function */
@@ -37,7 +79,7 @@ static int8_t header_t_less_than(void *a, void *b) {
 }
 
 heap_t *create_heap(uint32_t start, uint32_t end, uint32_t max, uint8_t supervisor, uint8_t readonly) {
-    heap_t *heap = (heap_t*)kmalloc(sizeof(heap_t)); //TODO: implement kmalloc
+    heap_t *heap = (heap_t*)kmalloc(sizeof(heap_t));
 
     ASSERT(start%0x1000 == 0);
     ASSERT(end%0x1000 == 0);
@@ -224,7 +266,7 @@ void free(void *p, heap_t *heap) {
     // Add header to free hole's index.
     int8_t add_to_free_hole = 1;
 
-    // If left-most thing is a footer, then:
+    // If left-most thing is a footer, then perform left unification
     footer_t *test_footer = (footer_t*) ((uint32_t)head - sizeof(footer_t));
     if(test_footer->magic == HEAP_MAGIC && 
         test_footer->header->is_hole == 1) {
@@ -235,5 +277,42 @@ void free(void *p, heap_t *heap) {
             add_to_free_hole = 0;
         }
     
+    header_t *test_header = (header_t*) ((uint32_t)foot + sizeof(footer_t));
+    if(test_header->magic == HEAP_MAGIC && test_header->is_hole) {
+        head->size += test_header->size; // Increase size
+        test_footer = (footer_t*) ((uint32_t)test_header * test_header->size - sizeof(footer_t));
+        foot = test_footer;
+        // remove this header from the index
+        uint32_t it = 0;
+        while((it < heap->index.size) && (lookup_ordered_list(it, &heap->index) != (void*)test_header))
+            it++;
+        
+        // Check if we found the item
+        ASSERT(it < heap->index.size);
+        // Then remove it
+        remove_ordered_list(it, &heap->index);
+    }
+
+    // Contract footer if it is at end address
+    if((uint32_t)foot+sizeof(footer_t) == heap->end_address) {
+        uint32_t old_len = heap->end_address-heap->start_address;
+        uint32_t new_len = contract((uint32_t)head - heap->start_address, heap);
+        // Check header size after resizing
+        if(head->size - (old_len-new_len) > 0) {
+            head->size -= old_len-new_len;
+            foot = (footer_t*) ((uint32_t)head + head->size - sizeof(footer_t));
+            foot->magic = HEAP_MAGIC;
+            foot->header = head;
+        } else { // Remove empty holes, this reduce fragmentation
+            uint32_t it = 0;
+            while((it < heap->index.size) && (lookup_ordered_list(it, &heap->index) != (void*)test_header))
+                it++;
+            // If nothing has been found, nothing will be removed
+            if(it < heap->index.size)
+                remove_ordered_list(it, &heap->index);
+        }
+    }
+    if(add_to_free_hole == 1)
+        insert_ordered_list((void*) head, &heap->index);
 
 }
\ No newline at end of file
diff --git a/kernel/drivers/kheap.h b/kernel/drivers/kheap.h
index b0acf2b..c8f1530 100644
--- a/kernel/drivers/kheap.h
+++ b/kernel/drivers/kheap.h
@@ -60,5 +60,12 @@ typedef struct {
 heap_t *create_heap(uint32_t start, uint32_t end, uint32_t max, uint8_t supervisor, uint8_t readonly);
 void *alloc(uint32_t size, uint8_t page_align, heap_t *heap);
 void free(void *p, heap_t *heap);
+// Public heap functions
+uint32_t kmalloc_init(uint32_t sz, int32_t align, uint32_t *phys);
+void kfree(void *p);
+uint32_t kmalloc_a(uint32_t sz);
+uint32_t kmalloc_p(uint32_t sz, uint32_t *phys);
+uint32_t kmalloc_ap(uint32_t sz, uint32_t *phys);
+uint32_t kmalloc(uint32_t sz);
 
 #endif
\ No newline at end of file
diff --git a/kernel/drivers/paging.c b/kernel/drivers/paging.c
new file mode 100644
index 0000000..75e37cb
--- /dev/null
+++ b/kernel/drivers/paging.c
@@ -0,0 +1,56 @@
+#include "paging.h"
+
+// Macros for bitset algorithms
+#define INDEX_FROM_BIT(a) (a/(8*4))
+#define OFFSET_FROM_BIT(a) (a%(8*4))
+
+// Kernel's page directory
+page_directory_t *kernel_directory = 0;
+// Current page directory
+page_directory_t *current_directory = 0;
+
+// Bitset of frames, used or free
+uint32_t *frames;
+uint32_t nframes;
+
+// From kheap.c
+extern uint32_t placement_address;
+extern heap_t *kheap;
+
+
+// Set a bit in the frame bitset
+static void set_frame(uint32_t frame_addr) {
+    uint32_t frame = frame_addr / 0x1000;
+    uint32_t idx = INDEX_FROM_BIT(frame);
+    uint32_t off = OFFSET_FROM_BIT(frame);
+    frames[idx] |= (0x1 << off);
+}
+
+// clear a bit in the frame bitset
+static void clear_frame(uint32_t frame_addr) {
+    uint32_t frame = frame_addr / 0x1000;
+    uint32_t idx = INDEX_FROM_BIT(frame);
+    uint32_t off = OFFSET_FROM_BIT(frame);
+    frames[idx] &= ~(0x1 << off);
+}
+
+// Test if a bit is set
+static uint32_t test_frame(uint32_t frame_addr) {
+    uint32_t frame = frame_addr / 0x1000;
+    uint32_t idx = INDEX_FROM_BIT(frame);
+    uint32_t off = OFFSET_FROM_BIT(frame);
+    return (frames[idx] & (0x1 << off));
+}
+
+// Find the first three frames
+static uint32_t first_frame() {
+    for(uint32_t i = 0; i < INDEX_FROM_BIT(nframes); i++) {
+        if(frames[i] != 0xFFFFFFFF) { // If nothing is free, exit
+            for(uint32_t j = 0; j < 32; j++) {
+                uint32_t to_test = 0x1 << j;
+                if (!(frames[i]&to_test))
+                    return i*4*8+j;
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/kernel/drivers/paging.h b/kernel/drivers/paging.h
new file mode 100644
index 0000000..21bfa84
--- /dev/null
+++ b/kernel/drivers/paging.h
@@ -0,0 +1,37 @@
+#ifndef PAGING_H
+#define PAGING_G
+
+#include <stdint.h>
+#include "isr.h"
+#include "kheap.h"
+
+typedef struct page {
+    uint32_t present : 1; // Presence in memory of single page
+    uint32_t fw : 1; // if set read write otherwise read only
+    uint32_t user : 1;  // Supervisored mode only if cleared
+    uint32_t accessed : 1; // true if page has been accessed since last refresh
+    uint32_t dirty : 1; // Same as above for write.
+    uint32_t unused : 7; // unused bits
+    uint32_t frame : 20; // Frame address (shifted to the right)
+} page_t;
+
+typedef struct page_table {
+    page_t pages[1024];
+} page_table_t;
+
+typedef struct page_directory {
+    page_table_t *tables[1024]; // array of pointers to page tables
+    uint32_t tables_physical[1024]; // address of tables physical. 
+    uint32_t physical_addr;
+} page_directory_t;
+
+// Setup the environment
+void init_paging();
+// Load specified page directory into CR3 register
+void switch_page_directory(page_directory_t *new);
+// Retrieve pointer to specified page address
+page_t *get_page(uint32_t addr, int32_t make, page_directory_t *dir);
+// Handle page faults
+void page_fault(registers_t regs);
+
+#endif
\ No newline at end of file