diff --git a/kernel/cpu/multiboot.asm b/kernel/cpu/multiboot.asm
index b336997..3344761 100644
--- a/kernel/cpu/multiboot.asm
+++ b/kernel/cpu/multiboot.asm
@@ -16,5 +16,5 @@ head_s:
     ; Various flags
     dw 0 ; type
     dw 0 ; flags
-    dd 0 ; size
+    dd 8 ; size
 head_e:
\ No newline at end of file
diff --git a/kernel/drivers/tty.c b/kernel/drivers/tty.c
index e644f2f..1fe306d 100644
--- a/kernel/drivers/tty.c
+++ b/kernel/drivers/tty.c
@@ -92,37 +92,6 @@ void kprint_dec(uint32_t num) {
     kprint(c2);
 }
 
-void kprint_hex(uint32_t num) {
-    int32_t tmp;
-    uint8_t no_zeros = 1;
-
-    kprint((uint8_t*)"0x");
-
-    for(int32_t i = 28; i > 0; i -= 4) {
-        tmp = (num >> i) & 0xF;
-        if(tmp == 0 && no_zeros != 0)
-            continue;
-        
-        if(tmp >= 0xA) {
-            no_zeros = 0;
-            uint8_t *buf = (uint8_t*)tmp-0xA+'a';
-            kprint_c(buf, strlen(buf), WHITE, BLACK);
-        } else {
-            uint8_t *buf = (uint8_t*)tmp+'a';
-            kprint_c(buf, strlen(buf), WHITE, BLACK);
-        }
-    }
-
-    tmp = num & 0xF;
-    if(tmp >= 0xA) {
-        uint8_t *buf = (uint8_t*)tmp-0xA+'a';
-        kprint_c(buf, strlen(buf), WHITE, BLACK);
-    } else {
-        uint8_t *buf = (uint8_t*)tmp+'a';
-        kprint_c(buf, strlen(buf), WHITE, BLACK);
-    }
-}
-
 void init_prompt() {
     uint8_t *prompt = (uint8_t*)"\nring0@iceOS-$ ";
     kprint_c(prompt, strlen(prompt), LIGHT_RED, BLACK);
diff --git a/kernel/drivers/tty.h b/kernel/drivers/tty.h
index c23330f..67cb7e2 100644
--- a/kernel/drivers/tty.h
+++ b/kernel/drivers/tty.h
@@ -54,7 +54,6 @@ void backspace();
 void kprint_c(uint8_t *buf, uint32_t len, uint8_t fg, uint8_t bg);
 void kprint(uint8_t *buf);
 void kprint_dec(uint32_t num);
-void kprint_hex(uint32_t num);
 void init_prompt();
 void clear_prompt();
 void clear_row(uint8_t row);
diff --git a/kernel/kernel_main.c b/kernel/kernel_main.c
index 9167dd8..332c3ef 100644
--- a/kernel/kernel_main.c
+++ b/kernel/kernel_main.c
@@ -10,8 +10,11 @@
 #include "drivers/idt.h"
 #include "drivers/timer.h"
 #include "drivers/keyboard.h"
+#include "mem/paging.h"
+#include "mem/kheap.h"
 #include "shell/shell.h"
 #include "libc/stdio.h"
+#include "libc/multiboot.h"
 
 #include <stdint.h>
 
@@ -34,6 +37,11 @@ void kernel_main() {
     init_keyboard(); // Initialize keyboard driver
     printf_color("\n[INFO]", LIGHT_CYAN, BLACK);
     printf_color("   - Loaded PS/2 driver", WHITE, BLACK);
+
+    init_paging();
+    printf_color("\n[INFO]", LIGHT_CYAN, BLACK);
+    printf_color("   - Loaded Paging", WHITE, BLACK);
+
     
     /* printf_color("\n[TEST]", LIGHT_BROWN, BLACK); // Testing heap
     printf_color("   - Allocating heap blocks..\n", LIGHT_BROWN, BLACK);
diff --git a/kernel/libc/multiboot.h b/kernel/libc/multiboot.h
new file mode 100644
index 0000000..4c8105d
--- /dev/null
+++ b/kernel/libc/multiboot.h
@@ -0,0 +1,117 @@
+
+/* multiboot.h - the header for Multiboot */
+/* Copyright (C) 1999, 2001  Free Software Foundation, Inc.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */
+
+/* Macros. */
+
+/* The magic number for the Multiboot header. */
+#define MULTIBOOT_HEADER_MAGIC          0x1BADB002
+
+/* The flags for the Multiboot header. */
+#ifdef __ELF__
+# define MULTIBOOT_HEADER_FLAGS         0x00000003
+#else
+# define MULTIBOOT_HEADER_FLAGS         0x00010003
+#endif
+
+/* The magic number passed by a Multiboot-compliant boot loader. */
+#define MULTIBOOT_BOOTLOADER_MAGIC      0x2BADB002
+
+/* The size of our stack (16KB). */
+#define STACK_SIZE                      0x4000
+
+/* C symbol format. HAVE_ASM_USCORE is defined by configure. */
+#ifdef HAVE_ASM_USCORE
+# define EXT_C(sym)                     _ ## sym
+#else
+# define EXT_C(sym)                     sym
+#endif
+
+#ifndef ASM
+/* Do not include here in boot.S. */
+
+/* Types. */
+
+/* The Multiboot header. */
+typedef struct multiboot_header
+{
+ unsigned long magic;
+ unsigned long flags;
+ unsigned long checksum;
+ unsigned long header_addr;
+ unsigned long load_addr;
+ unsigned long load_end_addr;
+ unsigned long bss_end_addr;
+ unsigned long entry_addr;
+} multiboot_header_t;
+
+/* The symbol table for a.out. */
+typedef struct aout_symbol_table
+{
+ unsigned long tabsize;
+ unsigned long strsize;
+ unsigned long addr;
+ unsigned long reserved;
+} aout_symbol_table_t;
+
+/* The section header table for ELF. */
+typedef struct elf_section_header_table
+{
+ unsigned long num;
+ unsigned long size;
+ unsigned long addr;
+ unsigned long shndx;
+} elf_section_header_table_t;
+
+/* The Multiboot information. */
+typedef struct multiboot_info
+{
+ unsigned long flags;
+ unsigned long mem_lower;
+ unsigned long mem_upper;
+ unsigned long boot_device;
+ unsigned long cmdline;
+ unsigned long mods_count;
+ unsigned long mods_addr;
+ union
+ {
+   aout_symbol_table_t aout_sym;
+   elf_section_header_table_t elf_sec;
+ } u;
+ unsigned long mmap_length;
+ unsigned long mmap_addr;
+} multiboot_info_t;
+
+/* The module structure. */
+typedef struct module
+{
+ unsigned long mod_start;
+ unsigned long mod_end;
+ unsigned long string;
+ unsigned long reserved;
+} module_t;
+
+/* The memory map. Be careful that the offset 0 is base_addr_low
+  but no size. */
+typedef struct memory_map
+{
+ unsigned long size;
+ unsigned long base_addr_low;
+ unsigned long base_addr_high;
+ unsigned long length_low;
+ unsigned long length_high;
+ unsigned long type;
+} memory_map_t;
+
+#endif /* ! ASM */
\ No newline at end of file
diff --git a/kernel/mem/kheap.c b/kernel/mem/kheap.c
index fecbf0c..607716e 100644
--- a/kernel/mem/kheap.c
+++ b/kernel/mem/kheap.c
@@ -140,7 +140,7 @@ heap_t *create_heap(uint32_t start, uint32_t end_addr, uint32_t max, uint8_t sup
     heap->readonly = readonly;
 
     header_t *hole = (header_t*)start;
-    hole->size = end - start;
+    hole->size = end_addr - start;
     hole->magic = HEAP_MAGIC;
     hole->is_hole = 1;
     insert_ordered_array((void*)hole, &heap->index);
@@ -194,4 +194,125 @@ void *alloc(uint32_t size, uint8_t page_align, heap_t *heap) {
         // Now we have enough space, so recall this function again
         return alloc(size, page_align, heap);
     }
+
+    header_t *origin_hole_head = (header_t*)lookup_ordered_array(it, &heap->index);
+    uint32_t origin_hole_p = (uint32_t)origin_hole_head;
+    uint32_t origin_hole_s = origin_hole_head->size;
+    // Check if we should split the hole into two parts
+    if(origin_hole_s-new_size < sizeof(header_t)+sizeof(footer_t)) {
+        // Increase the requested size to the size of the hole we found
+        size += origin_hole_s-new_size;
+        new_size = origin_hole_s;
+    }
+    
+    // Check if we need to page-align data
+    if(page_align && origin_hole_p&0xFFFFF000) {
+        uint32_t new_loc = origin_hole_p + 0x1000 - (origin_hole_p&0xFFF) - sizeof(header_t);
+        header_t *hole_header = (header_t*)origin_hole_p;
+        hole_header->size = 0x1000 - (origin_hole_p&0xFFF) - sizeof(header_t);
+        hole_header->magic = HEAP_MAGIC;
+        hole_header->is_hole = 1;
+        footer_t *hole_footer = (footer_t*)((uint32_t)new_loc - sizeof(header_t));
+        hole_footer->magic = HEAP_MAGIC;
+        hole_footer->header = hole_header;
+        origin_hole_p = new_loc;
+        origin_hole_s = origin_hole_s - hole_header->size;
+    } else
+        remove_ordered_array(it, &heap->index); // Remove hole, since we don't need it anymore
+
+    // Rewrite original header
+    header_t *block_head = (header_t*)origin_hole_p;
+    block_head->magic = HEAP_MAGIC;
+    block_head->is_hole = 0;
+    block_head->size = new_size;
+    // and the footer
+    footer_t *block_foot = (footer_t*)(origin_hole_p + sizeof(header_t) + size);
+    block_foot->magic = HEAP_MAGIC;
+    block_foot->header = block_head;
+
+    // Check if we need to write a new hole after the allocated block
+    if(origin_hole_s - new_size > 0) {
+        header_t *hole_head = (header_t*)(origin_hole_p + sizeof(header_t) + size + sizeof(footer_t));
+        hole_head->magic = HEAP_MAGIC;
+        hole_head->is_hole = 1;
+        hole_head->size = origin_hole_s - new_size;
+        footer_t *hole_foot = (footer_t*)((uint32_t)hole_head + origin_hole_s - new_size - sizeof(footer_t));
+        if((uint32_t)hole_foot < heap->end_adddress) {
+            hole_foot->magic = HEAP_MAGIC;
+            hole_foot->header = hole_head;
+        }
+        // Add new hole to the data structure
+        insert_ordered_array((void*)hole_head, &heap->index);
+    }
+
+    // Return the block header
+    return (void*)((uint32_t)block_head+sizeof(header_t));
+}
+
+void free(void *p, heap_t *heap) {
+    // Check null pointers
+    if(p == 0)
+        return;
+
+    // Retrieve data
+    header_t *head = (header_t*)((uint32_t)p - sizeof(header_t));
+    footer_t *foot = (footer_t*)((uint32_t)head + head->size - sizeof(footer_t));
+
+    // TODO: assert
+
+    head->is_hole = 1; // Make this a hole
+    int8_t add_to_free_hole = 1; // Add this header to free holes
+
+    // Left unify
+    footer_t *test_foot = (footer_t*)((uint32_t)head - sizeof(footer_t));
+    if(test_foot->magic == HEAP_MAGIC && test_foot->header->is_hole == 1 ) {
+        uint32_t cache_s = head->size; // Store current size
+        head = test_foot->header; // Rewrite header into new one
+        foot->header = head; // Point footer to the new header
+        head->size += cache_s; // Increase size
+        add_to_free_hole = 0; // Header already in the structure.
+    }
+
+    // Right unify
+    header_t *test_head = (header_t*)((uint32_t)foot + sizeof(footer_t));
+    if(test_head->magic == HEAP_MAGIC && test_head->is_hole) {
+        head->size += test_head->size; // Increase size
+        test_foot = (footer_t*)((uint32_t)test_foot + test_head->size - sizeof(footer_t));
+        foot = test_foot;
+    // Find and remove this header from the structure
+    uint32_t it = 0;
+    while((it < heap->index.size) && (lookup_ordered_array(it, &heap->index) != (void*)test_head))
+        it++;
+    
+    // TODO: ASSERTION
+    // Check if we actually found something
+    // Remove that item
+    remove_ordered_array(it, &heap->index);
+    }
+
+    // If footer is located at the end, we can contract the heap
+    if((uint32_t)foot+sizeof(footer_t) == heap->end_adddress) {
+        uint32_t old_len = heap->end_adddress-heap->start_address;
+        uint32_t new_len = contract((uint32_t)head - heap->start_address, heap);
+        // Check dimensions after resizing
+        if(head->size - (old_len-new_len) > 0) {
+            // Dimensions is still a positive value, so we can resize
+            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 block from the structure
+            uint32_t it = 0;
+            while((it < heap->index.size) && (lookup_ordered_array(it, &heap->index) != (void*)test_head))
+                it++;
+            // If we didn't find that block we haven't nothing to remove
+            if(it < heap->index.size)
+                remove_ordered_array(it, &heap->index);
+        }
+    }
+
+    // If required by the user, add that block to the structure
+    if(add_to_free_hole == 1)
+        insert_ordered_array((void*)head, &heap->index);
 }
\ No newline at end of file
diff --git a/kernel/mem/kheap.h b/kernel/mem/kheap.h
index 38ece9b..90dfd5d 100644
--- a/kernel/mem/kheap.h
+++ b/kernel/mem/kheap.h
@@ -50,7 +50,7 @@ typedef struct {
     ordered_array_t index;
     uint32_t start_address; // Begin of allocated space
     uint32_t end_adddress; // End of allocated space
-    uint32_t max_address; // Maximum size heap ca be expanded to
+    uint32_t max_address; // Maximum size heap can be expanded to
     uint8_t supervisor;
     uint8_t readonly;
 } heap_t;
diff --git a/kernel/mem/ordered_array.c b/kernel/mem/ordered_array.c
index e18cbe3..a5b3522 100644
--- a/kernel/mem/ordered_array.c
+++ b/kernel/mem/ordered_array.c
@@ -36,16 +36,16 @@ void destroy_ordered_array(ordered_array_t *array) {
 
 void insert_ordered_array(type_t item, ordered_array_t *array) {
     uint32_t it = 0;
-    while(it < array->size && array->less_than(array->array[it], it));
+    while(it < array->size && array->less_than(array->array[it], item))
         it++;
     if(it == array->size)
         array->array[array->size++] = item;
     else {
         type_t tmp = array->array[it];
-        array->array[it] = it;
+        array->array[it] = item;
         while(it < array->size) {
             it++;
-            type_t tmp = array->array[it];
+            type_t tmp2 = array->array[it];
             array->array[it] = tmp;
             tmp = tmp2;
         }
diff --git a/kernel/mem/paging.c b/kernel/mem/paging.c
index d3f6de3..198d26c 100644
--- a/kernel/mem/paging.c
+++ b/kernel/mem/paging.c
@@ -1,6 +1,7 @@
 #include "paging.h"
 #include "../libc/string.h"
 #include "../libc/assert.h"
+#include "../libc/stdio.h"
 #include "../drivers/tty.h"
 
 // External definitions from kheap.c
@@ -68,20 +69,25 @@ void free_frame(page_t *page) {
 }
 
 void init_paging() {
+    // Setup frame allocation
+    nframes = PHYSICAL_MEM_SIZE / FRAME_SIZE;
+    frame_allocations = (uint32_t*)kmalloc(nframes/FRAME_ALLOCATIONS_SECTION_SIZE);
+    memset(frame_allocations, 0, nframes/FRAME_ALLOCATIONS_SECTION_SIZE);
+
+    // Setup page directory
     kernel_directory = (page_directory_t*)kmalloc_a(sizeof(page_directory_t));
     memset(kernel_directory, 0, sizeof(page_directory_t));
     current_directory = kernel_directory;
 
-    kernel_directory = (page_directory_t*)kmalloc_a(sizeof(page_directory_t));
-    memset(kernel_directory, 0, sizeof(page_directory_t));
-    current_directory = kernel_directory;
-
+    // Map heap pages
     for(uint32_t i = KHEAP_START; i < KHEAP_START + KHEAP_INITIAL_SIZE; i += FRAME_SIZE)
         get_page(i, 1, kernel_directory);
 
+    // Setup identity map
     for(uint32_t i  = 0; i < placement_addr + FRAME_SIZE; i += FRAME_SIZE)
         alloc_frame(get_page(i, 1, kernel_directory), 0, 0);
     
+    // Allocate heap pages
     for(uint32_t i = KHEAP_START; i < KHEAP_START+KHEAP_INITIAL_SIZE; i += FRAME_SIZE)
         alloc_frame(get_page(i, 1, kernel_directory), 0, 0);
 
@@ -121,17 +127,15 @@ void page_fault(registers_t regs) {
     asm volatile("mov %%cr2, %0" : "=r" (faulting_addr));
 
     // Gracefully print the error
-    kprint((uint8_t*)"Page fault! ( ");
+    kprint((uint8_t*)"\nPage fault! ( ");
     if(!(regs.err_code & 0x1))
-        kprint((uint8_t*)"Present");
+        kprint((uint8_t*)"Present ");
     if(regs.err_code & 0x2)
-        kprint((uint8_t*)"Read-Only");
+        kprint((uint8_t*)"Read-Only ");
     if(regs.err_code & 0x4)
-        kprint((uint8_t*)"User-Mode");
+        kprint((uint8_t*)"User-Mode ");
     if(regs.err_code & 0x8)
         kprint((uint8_t*)"Reserved");
-    kprint((uint8_t*)") at 0x");
-    kprint_hex(faulting_addr);
-    kprint((uint8_t*)"\n");
+    printf(") at %x\n", faulting_addr);
     PANIC("Page fault");
 }
\ No newline at end of file