diff --git a/bochsrc.bxrc b/bochsrc.bxrc
index 6de97b7..34c323d 100644
--- a/bochsrc.bxrc
+++ b/bochsrc.bxrc
@@ -10,7 +10,7 @@ floppy_bootsig_check: disabled=0
 floppya: type=1_44
 # no floppyb
 ata0: enabled=true, ioaddr1=0x1f0, ioaddr2=0x3f0, irq=14
-ata0-master: type=disk, path=".\disk.img", mode=flat, cylinders=0, heads=0, spt=0, sect_size=512, model="Stevia Disk", biosdetect=auto, translation=auto
+ata0-master: type=disk, path=".\disk.img", mode=flat, cylinders=0, heads=0, spt=0, sect_size=512, model="Stevia Disk", biosdetect=auto, translation=lba
 ata0-slave: type=none
 ata1: type=none
 ata1-master: type=none
diff --git a/include/BIOS/func/E820_memory_map.inc b/include/BIOS/func/E820_memory_map.inc
index 9cd9231..3447a47 100644
--- a/include/BIOS/func/E820_memory_map.inc
+++ b/include/BIOS/func/E820_memory_map.inc
@@ -18,6 +18,7 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_E820MEMORY_MAP
 
 ; Address Range Descriptor Structure
 ;
@@ -116,6 +117,7 @@ struc AddressRangeDescStruct_t
     .ExtType        resd 1
 endstruc
 
+ALIGN 4, db 0x90
 GetMemoryMap:
     __CDECL16_ENTRY
     push es         ; save segment register
@@ -158,4 +160,7 @@ GetMemoryMap:
 .endp:
     pop es
     __CDECL16_EXIT
-    ret
\ No newline at end of file
+    ret
+    
+%endif
+%define __INC_E820MEMORY_MAP
\ No newline at end of file
diff --git a/include/BIOS/func/a20enable.inc b/include/BIOS/func/a20enable.inc
index 0b18bf5..cde9a18 100644
--- a/include/BIOS/func/a20enable.inc
+++ b/include/BIOS/func/a20enable.inc
@@ -18,6 +18,7 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_A20ENABLE
 ;
 ;INT 0x15 Function 2400 - Disable A20
 ;Returns:
@@ -66,6 +67,7 @@
 ;    Bit 3 - power on password bytes (CMOS bytes 0x38-0x3f or 0x36-0x3f). 0: accessible, 1: inaccessible
 ;    Bits 4-5 - Manufacturer defined
 ;    Bits 6-7 - 00: HDD activity LED off; any other value is "on"
+ALIGN 4, db 0x90
 EnableA20:
     __CDECL16_ENTRY
     push ds
@@ -136,4 +138,7 @@ EnableA20:
     pop es
     pop ds
     __CDECL16_EXIT
-    ret
\ No newline at end of file
+    ret
+    
+%endif
+%define __INC_A20ENABLE
\ No newline at end of file
diff --git a/include/BIOS/func/ext_read.inc b/include/BIOS/func/ext_read.inc
index 6e137ce..a107c94 100644
--- a/include/BIOS/func/ext_read.inc
+++ b/include/BIOS/func/ext_read.inc
@@ -18,6 +18,7 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_EXT_READ
 ;Offset	Size	    Description
 ; 0	    1	    size of packet (16 bytes)
 ; 1	    1	    always 0
@@ -52,25 +53,28 @@ endstruc
 ; successfully transferred
 ;
 ;
-; uint8_t read_disk_raw(uint16_t buf_segment, uint16_t buf_offset, uint32_t lba)
-; TODO: this needs validation
+; uint8_t read_stage2_raw(uint16_t buf_segment, uint16_t buf_offset, 
+;                         uint32_t lba,
+;                         uint16_t count, uint16_t drive_num)
+ALIGN 4, db 0x90
 read_disk_raw:
     __CDECL16_ENTRY
-.func:  
+.func:                        
     mov ax, 0x10
     push ax             ; len = 16 bytes
     xor ax, ax
     push ax             ; val = 0
     mov ax, lba_packet
     push ax             ; dest = lba_packet address
-
-    call kmemset        ; uint8_t* kmemset(void* dest, uint8_t val, size_t len);
-    add sp, 0x6
+    call kmemset        
+    add sp, 0x06
 
     mov byte [lba_packet + LBAPkt_t.size], 0x10
-    mov word [lba_packet + LBAPkt_t.xfer_size], 0x0001
 
-    mov dword eax, [bp + 8]
+    mov ax, [bp + 12]
+    mov word [lba_packet + LBAPkt_t.xfer_size], ax
+
+    mov eax, [bp + 8]
     mov dword [lba_packet + LBAPkt_t.lower_lba], eax
 
     mov ax, [bp + 6]
@@ -81,10 +85,26 @@ read_disk_raw:
 
     mov si, lba_packet
     mov ah, 0x42
-    mov dl, byte [fat32_ebpb + FAT32_ebpb_t.drive_number_8]
+
+    ;BUG: still working on getting this passed down correctly, going to hard set it for now.
+    movzx dx, byte [bp + 14]                   
+    and dx, 0x00ff
+    ;xor dl, dl
+    ;mov dl, 0x80
+
     int 0x13
-    jnc read_disk_raw.endp
+    jnc .endf
+
+    %ifdef __STEVIA_MBR
+    ERROR MBR_ERROR_DISK_READ_ERR
+    %elifsef __STEVIA_VBR
+    ERROR VBR_ERROR_DISK_READ_ERR
+    %else
     ERROR STAGE2_MBR_DISK_READ_ERROR
-.endp:
+    %endif
+.endf:
     __CDECL16_EXIT
-    ret
\ No newline at end of file
+    ret
+
+%endif
+%define __INC_EXT_READ
\ No newline at end of file
diff --git a/include/BIOS/func/video.inc b/include/BIOS/func/video.inc
index 5a5e78e..33b0a6e 100644
--- a/include/BIOS/func/video.inc
+++ b/include/BIOS/func/video.inc
@@ -18,9 +18,11 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_VIDEO
 ; Sets output to 80x25 16 color text mode via BIOS call
 ; also clears screen
 ; void SetTextMode(void)
+ALIGN 4, db 0x90
 SetTextMode:
 .prolog:
     __CDECL16_ENTRY
@@ -39,6 +41,7 @@ SetTextMode:
     ret
 
 ; disables blinking text mode cursor
+ALIGN 4, db 0x90
 disable_cursor:
     __CDECL16_ENTRY
 .func:
@@ -51,4 +54,7 @@ disable_cursor:
 	out dx, al
 .endp:
     __CDECL16_EXIT
-    ret
\ No newline at end of file
+    ret
+    
+%endif
+%define __INC_VIDEO
\ No newline at end of file
diff --git a/include/cdecl16.inc b/include/cdecl16.inc
index b039985..13bdc50 100644
--- a/include/cdecl16.inc
+++ b/include/cdecl16.inc
@@ -21,6 +21,7 @@
 %macro __CDECL16_ENTRY 0
     push bp
     mov bp, sp
+    sub sp, 0x20
 
     push si
     push di
@@ -37,4 +38,4 @@
     mov sp, bp
     pop bp
 %endmacro
-%endif
+%endif
\ No newline at end of file
diff --git a/include/config.inc b/include/config.inc
index 3a780f6..db74252 100755
--- a/include/config.inc
+++ b/include/config.inc
@@ -18,8 +18,12 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_STEVIA_CONFIG
 
 %define SECTOR_SIZE             512
 %define STAGE2_SECTOR_COUNT    0x40
 ; 32 KiB
-%define MAX_STAGE2_BYTES      (SECTOR_SIZE * STAGE2_SECTOR_COUNT)
\ No newline at end of file
+%define MAX_STAGE2_BYTES      (SECTOR_SIZE * STAGE2_SECTOR_COUNT)
+
+%endif
+%define __INC_STEVIA_CONFIG
\ No newline at end of file
diff --git a/include/entry.inc b/include/entry.inc
index f295305..fa5e710 100755
--- a/include/entry.inc
+++ b/include/entry.inc
@@ -18,9 +18,13 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_ENTRY
 
 ; 8KiB from 0x2500 -> 0x500
 %define EARLY_STACK_START            0x2500
 %define MBR_ENTRY                    0x7A00
 %define VBR_ENTRY                    0x7C00
-%define STAGE2_ENTRY                 0x7E00
\ No newline at end of file
+%define STAGE2_ENTRY                 0x7E00
+
+%endif
+%define __INC_ENTRY
\ No newline at end of file
diff --git a/include/error_codes.inc b/include/error_codes.inc
index bc1999c..d1eb182 100755
--- a/include/error_codes.inc
+++ b/include/error_codes.inc
@@ -18,6 +18,7 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_ERROR_CODES
 
 ; Errors
 ; 12 Errors, 5 in use
@@ -76,4 +77,7 @@
 %define STEVIA_DEBUG_OK      'W'
 %define STEVIA_DEBUG_ERR      'X'
 %define STEVIA_DEBUG_UNIMPLEMENTED      'Y'
-%define STEVIA_DEBUG_HALT      'Z'
\ No newline at end of file
+%define STEVIA_DEBUG_HALT      'Z'
+
+%endif
+%define __INC_ERROR_CODES
\ No newline at end of file
diff --git a/include/fat32/bpb.inc b/include/fat32/bpb_offset_bx.inc
similarity index 98%
rename from include/fat32/bpb.inc
rename to include/fat32/bpb_offset_bx.inc
index 8200b34..e807556 100755
--- a/include/fat32/bpb.inc
+++ b/include/fat32/bpb_offset_bx.inc
@@ -18,6 +18,7 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_BPD_OFFSET_BX
 
 ; BPB Information
 ; Off.  Hex Off. Size   Description
@@ -88,4 +89,7 @@
 %define bsVolumeSerial      bx+0x43
 %define bsVolumeLabel       bx+0x47
 %define bsSystemIdent       bx+0x52
-;-- End EBPB
\ No newline at end of file
+;-- End EBPB
+
+%endif
+%define __INC_BPD_OFFSET_BX
\ No newline at end of file
diff --git a/include/fat32/fat32_structures.inc b/include/fat32/fat32_structures.inc
index 959b0e5..0bba2a5 100755
--- a/include/fat32/fat32_structures.inc
+++ b/include/fat32/fat32_structures.inc
@@ -18,6 +18,7 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_FAT32_STRUCT
 
 ; ## FAT32 Info ##
 ;   total_sectors = bsSectorsHuge
@@ -230,4 +231,5 @@ endstruc
 ; LFN == RO | HIDDEN | SYSTEM | VOLID == 0x0F
 %define FAT32_ATTR_LFN          0x0F
 
-
+%endif
+%define __INC_FAT32_STRUCT
\ No newline at end of file
diff --git a/include/fat32/fat32_func_old.inc b/include/fat32/fat32_sys.inc
similarity index 82%
rename from include/fat32/fat32_func_old.inc
rename to include/fat32/fat32_sys.inc
index 33175ed..3de49ab 100644
--- a/include/fat32/fat32_func_old.inc
+++ b/include/fat32/fat32_sys.inc
@@ -18,6 +18,13 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_FAT32_SYS
+
+%include "partition_table.inc"
+%include "fat32/bpb_offset_bx.inc"
+%include "fat32/fat32_structures.inc"
+
+ALIGN 4, db 0x90
 InitFATDriver:
     __CDECL16_ENTRY
 .func:
@@ -28,9 +35,14 @@ InitFATDriver:
     mov dword [fat32_state + FAT32_State_t.active_dir_cluster_32], eax
 
 .calc_active_part:
-    mov ax, word [partition_offset]
-    mov bx, partition_table
-    add bx, ax                                                           ; bx points to the partition that was booted from
+    mov bx, [partition_offset_ptr]
+    mov ax, [bx + 0]
+    mov bx, ax
+    mov ax, [bx + 0]                       
+
+    mov dx, partition_table
+    add dx, ax                                                           ; dx points to the partition that was booted from
+    mov bx, dx                                                           ; set bx, should point at our partition
 
     mov eax, dword [bx + PartEntry_t.lba_start]
     mov dword [fat32_state + FAT32_State_t.active_drive_lba_32], eax
@@ -72,6 +84,7 @@ InitFATDriver:
 ; returns first cluster of file if found
 ; halts/errors if file is not found
 ; uint32_t SearchFATDIR(uint8_t* SFN);
+ALIGN 4, db 0x90
 SearchFATDIR:
     __CDECL16_ENTRY
 .file_lookup:
@@ -173,9 +186,16 @@ SearchFATDIR:
 ; uint32_t NextCluster(uint32_t active_cluster);
 ; if eax >= 0x0FFFFFF8 then there are no more clusters (end of chain)
 ; if eax == 0x0FFFFFF7 then this is a cluster that is marked as bad
+ALIGN 4, db 0x90
 NextCluster:
     __CDECL16_ENTRY
 .func:
+    mov bx, [boot_drive_ptr]
+    mov ax, [bx + 0]
+    mov bx, ax
+    movzx ax, byte [bx + 0]                      ; 67h override would probably work but this is the 16bit way to do it
+    mov byte [bp - 2], al                        ; save boot drive as a local for easy access
+
     mov dword edx, [bp + 4]
     mov si, fat32_nc_data
 .calc_offset:
@@ -213,7 +233,14 @@ NextCluster:
     add eax, ecx        ; fat_sector + first_fat_sector
     mov dword [si + FAT32_NextClusterData_t.fat_sector], eax
 .load_fat_table:
-    ; load correct fat 
+    movzx ax, [bp - 2]
+    push ax
+
+    mov ax, 0x1
+    push ax
+
+    ; load correct fat
+    mov eax, dword [si + FAT32_NextClusterData_t.fat_sector]
     push dword eax
 
     mov ax, fat_buffer
@@ -221,10 +248,11 @@ NextCluster:
 
     xor ax, ax
     push ax
-    ; uint8_t read_disk_raw(uint16_t buf_segment, uint16_t buf_offset, uint16_t lower_lower_lba, uint16_t upper_lower_lba)
-    call read_disk_raw  ; read_disk_raw(0, fat_buffer, fat_sector)
-    add sp, 0x8
-
+    call read_disk_raw  
+    add sp, 0xC
+    ; uint8_t read_stage2_raw(uint16_t buf_segment, uint16_t buf_offset, 
+;                         uint32_t lba,
+;                         uint16_t count, uint16_t drive_num)
 .read_cluster:
 ; next_cluster = fat_buffer[entry_offset]
     mov ebx, dword [si + FAT32_NextClusterData_t.entry_offset]
@@ -235,6 +263,7 @@ NextCluster:
     ret
 
 ; uint32_t ClusterToLBA(uint32_t cluster)
+ALIGN 4, db 0x90
 ClusterToLBA:
     __CDECL16_ENTRY
 .func:
@@ -249,25 +278,42 @@ ClusterToLBA:
     ret
 
 ; uint8_t ReadFATCluster(uint16_t seg, uint16_t offset, uint32_t cluster)
+ALIGN 4, db 0x90
 ReadFATCluster:
     __CDECL16_ENTRY
 .func:
+    mov bx, [boot_drive_ptr]
+    mov ax, [bx + 0]
+    mov bx, ax
+    movzx ax, byte [bx + 0]                      ; 67h override would probably work but this is the 16bit way to do it
+    mov byte [bp - 2], al                        ; save boot drive as a local for easy access
+
     mov dword eax, [bp + 8]
     push dword eax
     call ClusterToLBA    ; uint32_t ClusterToLBA(uint32_t cluster)
     add sp, 0x4         ; eax == LBA
     
 
-    push dword eax      ;uint32_t lba
+    movzx ax, [bp - 2]
+    push ax
 
-    mov dx, [bp + 4]    ; seg
-    push dx             ; uint16_t buf_offset
+    mov ax, 0x1
+    push ax
 
-    mov dx, [bp + 6]    ; offset
-    push dx             ; unit16_t segment
+    ; load correct fat
+    mov eax, dword [si + FAT32_NextClusterData_t.fat_sector]
+    push dword eax
 
-    call read_disk_raw ; uint8_t read_disk_raw(uint16_t buf_segment, uint16_t buf_offset, uint32_t lba)
-    add sp, 0x8
+    mov ax, fat_buffer
+    push ax
+
+    xor ax, ax
+    push ax
+    call read_disk_raw  
+    add sp, 0xC
 .endp:
     __CDECL16_EXIT
     ret
+
+%endif
+%define __INC_FAT32_SYS
\ No newline at end of file
diff --git a/include/kmem_func.inc b/include/kmem_func.inc
index e69ca31..93f544d 100644
--- a/include/kmem_func.inc
+++ b/include/kmem_func.inc
@@ -17,15 +17,18 @@
 ; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
+
+%ifndef __INC_KMEM_FUNC
 %include 'cdecl16.inc'
 
-; uint8_t* kmemset(void* dest, uint8_t val, uint8_t len);
+; uint8_t* kmemset_byte(void* dst, uint8_t val, uint16_t len);
+ALIGN 4, db 0x90
 kmemset:
     __CDECL16_ENTRY
  .func:
     mov     cx, [bp + 8]   ; size_t len
     mov     al, [bp + 6]   ; uint8_t val
-    mov     di, [bp + 4]   ; void * ptr
+    mov     di, [bp + 4]   ; void * dst
 
     cld
     rep     stosb
@@ -36,6 +39,7 @@ kmemset:
 
 ; uint8_t* kmemset(uint8_t* dest, uint8_t* src, uint8_t len);
 ; not overlap safe
+ALIGN 4, db 0x90
 kmemcpy:
     __CDECL16_ENTRY
 .func:
@@ -48,4 +52,7 @@ kmemcpy:
     mov ax, di              ; return pointer to dest
 .endf:
     __CDECL16_EXIT
-    ret
\ No newline at end of file
+    ret
+
+%endif
+%define __INC_KMEM_FUNC
\ No newline at end of file
diff --git a/include/memory.inc b/include/mem.inc
similarity index 93%
rename from include/memory.inc
rename to include/mem.inc
index 68e1305..ca8ac77 100755
--- a/include/memory.inc
+++ b/include/mem.inc
@@ -18,6 +18,8 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_MEM
+
 
 ; ## Generic Low mem map (from osdev wiki) ##
 ; start 	        end 	    size 	        type 	                    description
@@ -54,12 +56,12 @@
 
 ; copy of FAT32 BPB, 33 bytes (+1 to the next value to align to uint16_t)
 ;0x3048
-%define fat32_bpb             0x3050
-%define fat32_bpb_SIZE        33
+%define fat32_bpb             0x304A
+%define fat32_bpb_SIZE        36
 
 ; copy of FAT32 EBPB, 54 bytes
 ;0x306A
-%define fat32_ebpb            0x30A0
+%define fat32_ebpb            0x306E
 %define fat32_ebpb_SIZE       54
 
 ; FAT32 FSInfo, 512 bytes
@@ -69,18 +71,18 @@
 
 ; some stored state for the fat32 driver
 ;0x32A2
-%define fat32_state           0x32B0
+%define fat32_state           0x34B0
 %define fat32_state_SIZE      32
 
 ; next free space is 0x32D0
-%define fat32_nc_data         0x32D0
+%define fat32_nc_data         0x35D0
 %define fat32_nc_data_size    16
 
 ; lba_packet for raw_disk_read
 %define lba_packet            0x4000
 
 %define BIOSMemoryMap         0x4200
-%define SteviaInfo            0x4400
+%define SteviaInfo            0x5200
 
 
 ; High memory addresses for loading kernel (for use with unreal mode and 32bit override)
@@ -110,8 +112,5 @@ struc SteviaInfoStruct_t
     .EBPBDataPtr       resd 1
 endstruc
 
-%macro DEBUG_HCF 0
-    DEBUG_LOOP:
-        hlt
-        jmp short DEBUG_LOOP
-%endmacro
\ No newline at end of file
+%endif
+%define __INC_MEM
\ No newline at end of file
diff --git a/include/partition_table.inc b/include/partition_table.inc
index 7f149fa..ddd9ab6 100755
--- a/include/partition_table.inc
+++ b/include/partition_table.inc
@@ -18,6 +18,8 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_PART_TABLE
+
 ; Partition table entry format
 ; Off.  Size.       Description
 ;0x00 	  1 	Drive attributes (bit 7 set = active or bootable)
@@ -41,4 +43,7 @@ struc PartTable_t
     .partition2 resb 16
     .partition3 resb 16
     .partition4 resb 16
-endstruc
\ No newline at end of file
+endstruc
+
+%endif
+%define __INC_PART_TABLE
\ No newline at end of file
diff --git a/include/util/error_func.inc b/include/util/error_func.inc
index cbd238a..d2a4901 100644
--- a/include/util/error_func.inc
+++ b/include/util/error_func.inc
@@ -18,13 +18,23 @@
 ; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 ; SOFTWARE.
 
+%ifndef __INC_ERROR_FUNC
+
 %macro ERROR 1
     xor ax, ax
     mov al, %1
     jmp error
 %endmacro
 
+%macro DEBUG_HCF 0
+    DEBUG_LOOP:
+        cli
+        hlt
+        jmp short DEBUG_LOOP
+%endmacro
+
 ; pass error as ascii character in al, errors a-zA-Z or 0-9
+ALIGN 4, db 0x90
 error:
     ; fs = 0xb800 => fs:0x0000 = 0xb8000
     mov dx, 0xB800
@@ -47,4 +57,7 @@ error:
 
 .stop:
     hlt
-    jmp short error.stop
\ No newline at end of file
+    jmp short error.stop
+
+%endif
+%define __INC_ERROR_FUNC
\ No newline at end of file
diff --git a/src/mbr/mbr.nasm b/src/mbr/mbr.nasm
index f65a202..e4450a8 100755
--- a/src/mbr/mbr.nasm
+++ b/src/mbr/mbr.nasm
@@ -23,17 +23,37 @@
 [CPU KATMAI]
 jmp short init
 nop
-%include "cdecl16.inc"              ; include calling convention macros for cdecl16
-%include "entry.inc"                ; defines with entry points for various stages & stack
-init:
-    cli                             ; We do not want to be interrupted
 
-    xor ax, ax                      ; 0 AX
-    mov ds, ax                      ; Set segment registers to 0
-    mov es, ax                      ; *
-    
-    mov ss, ax                      ; Set Stack Segment to 0
-    mov sp, EARLY_STACK_START             
+; ###############
+;
+; Headers/Includes/Definitions
+;
+; ###############
+
+%define __STEVIA_MBR
+
+%include "cdecl16.inc"
+%include "entry.inc"
+%include "config.inc"
+%include "mem.inc"
+%include "error_codes.inc"
+%include "partition_table.inc"
+
+; ###############
+; End Section
+; ###############
+
+ALIGN 4, db 0x90
+init:
+    cli                         ; We do not want to be interrupted
+
+    xor ax, ax                  ; 0 AX
+    mov ds, ax                  ; Set segment registers to 0
+
+    mov ss, ax                  ; Set Stack Segment to 0
+    mov sp, EARLY_STACK_START   ; Setup stack
+    mov bp, sp                  ; base ptr = stack ptr
+    sub sp, 0x20                ; local varible space             
 
     mov ch, 0x01                    ; 256 WORDs in MBR (512 bytes), 0x0100 in cx
     mov si, 0x7C00                  ; Current MBR Address (loaded here by BIOS)
@@ -43,16 +63,28 @@ init:
     sti
 
     jmp 0:main
-    nop
 
-%include "config.inc"
-%include "memory.inc"
+; ###############
+;
+; Extra/Shared Functions
+;
+; ###############
+
 %include "kmem_func.inc"
-%include "partition_table.inc"
-%include "errors.inc"
+%include "util/error_func.inc"
 
+; ###############
+; End Section
+; ###############
+
+;
+; bp - 2 : uint8_t boot_drive
+; bp - 4 : uint16_t part_offset
+;
+
+ALIGN 4, db 0x90
 main:
-    mov [boot_drive], dl            ; BIOS passes drive number in DL
+    mov byte [bp - 2], dl            ; BIOS passes drive number in DL
 
     .check_disk:
         cmp dl, 0x80
@@ -62,7 +94,7 @@ main:
     .check_extentions:
         xor ax, ax
         mov bx, 0x55AA
-        mov dl, byte [boot_drive]
+        mov dl, byte [bp - 2]
         int 0x13
         jnc main.find_active
         ERROR MBR_ERROR_NO_INT32E                         ; no extended function support
@@ -83,25 +115,30 @@ main:
         mov ax, bx
         sub ax, DiskSig                         ; leaves us with the offset relative from start of table
                                                 ; this gives us an offset from the begining of the partition table
-        mov word [part_offset], ax              ; update part_offset
+        mov word [bp - 4], ax                   ; update part_offset
     .read_data:
-        ;uint8_t read_disk_raw(size_t count, uint16_t buf_segment, uint16_t buf_offset, 
-        ;                      uint16_t lower_lower_lba, uint16_t upper_lower_lba)
-        mov eax, dword [bx + PartEntry_t.lba_start]
-        ror eax, 16
-        push ax
+        movzx ax, byte [bp - 2]
+        push ax                                            ; drive_num
 
-        ror eax, 16
-        push ax         
+        mov ax, 0x1
+        push ax                                            ; count
 
-        mov ax, VBR_ENTRY
-        push ax
+        mov dword eax, dword [bx + PartEntry_t.lba_start]
+        push dword eax                                     ; lba
 
         xor ax, ax
-        push ax        ; segment = 0  
+        push ax                                            ; offset = 0
+
+        mov ax, VBR_ENTRY
+        shr ax, 4
+        push ax                                            ; segment = 7C0
+
+        ; uint8_t read_stage2_raw(uint16_t buf_segment, uint16_t buf_offset, 
+        ;                         uint32_t lba,
+        ;                         uint16_t count, uint16_t drive_num)
+        call read_disk_raw
+        add sp, 0xC
 
-        call read_vbr_raw
-        add sp, 0x8
 
         jnc main.goto_vbr
         ERROR MBR_ERROR_DISK_READ_ERR                     ; error in LBA read
@@ -120,86 +157,16 @@ main:
         call kmemcpy                            ; copy partition table to memory
         add sp, 0x6
 
-        xor ah, ah                              ; Set Video mode BIOS function
-        mov al, 0x02                            ; 16 color 80x25 Text mode
-        int 0x10                                ; Call video interrupt
-
-        mov ah, 0x05                            ; Select active display page BIOS function
-        xor al, al                              ; page 0
-        int 0x10                                ; call video interrupt
-
-        mov si, word [part_offset]
-        mov dl, byte [boot_drive]
+        mov si, word [bp - 4]
+        mov dl, byte [bp - 2]
         jmp 0:0x7C00
 
-; Wrapper for AH=0x42 INT13h (Extended Read)
+; ###############
 ;
-; BIOS call details
-; AH = 42h
-; DL = drive number
-; DS:SI -> disk address packet
+; BIOS Functions
 ;
-; Return:
-; CF clear if successful
-; AH = 00h
-; CF set on error
-; AH = error code
-; disk address packet's block count field set to number of blocks
-; successfully transferred
-;
-;
-; uint8_t read_disk_raw(uint16_t buf_segment, uint16_t buf_offset, uint16_t lower_lower_lba, uint16_t upper_lower_lba)
-read_vbr_raw:
-    __CDECL16_ENTRY
-.func:                        
-    mov ax, 0x10
-    push ax             ; len = 16 bytes
-    xor ax, ax
-    push ax             ; val = 0
-    mov ax, lba_packet
-    push ax             ; dest = lba_packet address
-    call kmemset        
-    add sp, 0x06
-
-    mov byte [lba_packet + LBAPkt_t.size], 0x10
-    mov word [lba_packet + LBAPkt_t.xfer_size], 0x0001
-
-    mov ax, [bp + 10]
-    shl eax, 16
-    mov ax, [bp + 8]
-    mov dword [lba_packet + LBAPkt_t.lower_lba], eax
-
-    mov ax, [bp + 6]
-    mov word [lba_packet + LBAPkt_t.offset], ax
-
-    mov ax, [bp + 4]
-    mov word [lba_packet + LBAPkt_t.segment], ax
-
-    mov si, lba_packet
-    mov ah, 0x42
-    mov dl, byte [boot_drive]
-    int 0x13
-    jnc .endf
-
-    ERROR MBR_ERROR_INT13h_EREAD_ERR
-.endf:
-    __CDECL16_EXIT
-    ret
-
-; #############
-;
-; Locals
-;
-; #############
-
-boot_drive:
-    db 0x00
-mbr_reserved1:
-    db 0x00
-
-; OFFSET from BEGINING of partition table, ie. DiskSig (-6 from PartEntry1)
-part_offset:
-    dw 0x0000
+; ###############
+%include 'BIOS/func/ext_read.inc'
 
 %assign bytes_remaining (440 - ($ - $$))
 %warning MBR has bytes_remaining bytes remaining for code (MAX: 440 bytes)
diff --git a/src/stage2/stage2.nasm b/src/stage2/stage2.nasm
index 5fad4a8..f014125 100755
--- a/src/stage2/stage2.nasm
+++ b/src/stage2/stage2.nasm
@@ -23,11 +23,27 @@
 [CPU KATMAI]
 jmp short init
 nop
-
 ; boot drive in dl
 ; active partition offset in si
+
+; ###############
+;
+; Headers/Includes/Definitions
+;
+; ###############
+%define __STEVIA_STAGE2
+
+
 %include "cdecl16.inc"
 %include "entry.inc"
+%include "config.inc"
+%include "mem.inc"
+%include "error_codes.inc"
+
+; ###############
+; End Section
+; ###############
+ALIGN 4, db 0x90
 init:
     cli                         ; We do not want to be interrupted
 
@@ -37,32 +53,36 @@ init:
     mov fs, ax                  ; *
     mov gs, ax                  ; *
 
-    mov ss, ax                  ; Set Stack Segment to 0
+    mov ss, ax                        ; Set Stack Segment to 0
     mov sp, EARLY_STACK_START         ; Set Stack Pointer
-
-    add sp, 0x4
-    mov ax, 0xDEAD
-    push word ax
-    mov ax, 0xBEEF
-    push word ax             ; mark top of stack for debuging
-
     mov bp, sp
+    sub sp, 0x20                      ; 32 bytes for local varibles
+
     sti
 
     jmp 0:main
 
-%include "config.inc"
-%include "error_codes.inc"
-%include "memory.inc"
-%include "kmem_func.inc"
-%include "partition_table.inc"
 
-%include "fat32/bpb.inc"
-%include "fat32/fat32_structures.inc"
+; ###############
+;
+; Extra/Shared Functions
+;
+; ###############
+
+%include "kmem_func.inc"
+%include "util/error_func.inc"
+
+; ###############
+; End Section
+; ###############
 
 main:
-    mov byte [fat32_ebpb + FAT32_ebpb_t.drive_number_8], dl
-    mov word [partition_offset], si
+    lea ax, [bp - 2]
+    mov [boot_drive_ptr], ax
+    lea ax, [bp - 4]
+    mov [partition_offset_ptr], ax
+    mov byte [bp - 2], dl               ; boot_drive (probably 0x80)
+    mov word [bp - 4], si               ; partition_offset
 
     mov eax, dword [STAGE2_SIG]
     cmp eax, 0xDEADBEEF
@@ -138,7 +158,7 @@ hcf:
 ;
 ; ###############
 
-%include 'fat32/fat32_func_old.inc'
+%include 'fat32/fat32_sys.inc'
 
 ; ###############
 ;
@@ -306,18 +326,13 @@ NewLine_cstr:
     db StrCRLF_NUL
 BootTarget_str:
     db "BOOTI686BIN"
-
-; #############
-;
-; Locals
-;
-; #############
-
-partition_offset:
+boot_drive_ptr:
+    db 0x00
+stage2_resb_1:
+    db 0x00
+partition_offset_ptr:
     dw 0x0000
 
-
-
 ; GDT documentation below:
 ;
 ;    Pr: Present bit. This must be 1 for all valid selectors.
diff --git a/src/vbr/vbr.nasm b/src/vbr/vbr.nasm
index 37759ca..2ffdd30 100755
--- a/src/vbr/vbr.nasm
+++ b/src/vbr/vbr.nasm
@@ -30,36 +30,59 @@ phy_ebpb_start:
 ; fill eBPB area with 0x00 since we skip writing this part to disk
 times 54 db 0x00
 
+; ###############
+;
+; Headers/Includes/Definitions
+;
+; ###############
+%define __STEVIA_VBR
+
+
 %include "cdecl16.inc"
 %include "entry.inc"
+%include "config.inc"
+%include "mem.inc"
+%include "error_codes.inc"
+%include "fat32/bpb_offset_bx.inc"
+
+; ###############
+; End Section
+; ###############
+
+ALIGN 4, db 0x90
 init:
     cli                         ; We do not want to be interrupted
 
     xor ax, ax                  ; 0 AX
     mov ds, ax                  ; Set segment registers to 0
-    mov es, ax                  ; *
 
     mov ss, ax                  ; Set Stack Segment to 0
-    mov sp, EARLY_STACK_START         ; Setup stack
+    mov sp, EARLY_STACK_START   ; Setup stack
     mov bp, sp                  ; base ptr = stack ptr
+    sub sp, 0x20                ; local varible space
 
     mov bx, VBR_ENTRY           ; move Bx to the new start of the initial boot sector
     sti                         ; all done with inital setup and relocation, reenable interupts
 
     jmp 0:main                  ; fix up cs:ip just in case and jump to relocated code
-    nop
 
-%include "config.inc"
-%include "memory.inc"
+; ###############
+;
+; Extra/Shared Functions
+;
+; ###############
+
 %include "kmem_func.inc"
-%include "partition_table.inc"
-%include "errors.inc"
+%include "util/error_func.inc"
 
-%include "fat32/bpb.inc"
+; ###############
+; End Section
+; ###############
 
+ALIGN 4, db 0x90
 main:
-    mov [bsDriveNumber], dl     ; BIOS passes drive number in DL
-    mov [partition_offset], si  ; save passed partition entry offset
+    mov byte [bp - 2], dl                 ; boot_drive
+    mov [bp - 4], si                 ; part_offset
 
 .check_FAT_size:                     ; we only support a very specific setup of FAT32
     cmp dword [bsSectorHuge], 0      ; SectorsHuge will not be set if FAT12/16
@@ -69,24 +92,39 @@ main:
 
 ; read sectors 1-63 to stage2 entry point
 .load_stage2:
-    xor ax, ax
-    push ax         ; upper_lower_lba = 0
+    mov ax, (fat32_bpb_SIZE + fat32_ebpb_SIZE)         ; size in byte
+    push ax
+    mov ax, (phy_bpb_start - 0x3)                      ; start of bpb - 0x3 for the jump short main at the start
+    push ax
+    mov ax, fat32_bpb                                  ; defined in memory.inc, destination
+    push ax
+    call kmemcpy                                       ; copy bpb to memory
+    add sp, 0x6
 
-    mov ax , 1
-    push ax         ; lower_lower_lba = 1
+    mov bx, fat32_bpb                                  ; bx now points to aligned memory structure
+
+    movzx ax, byte [bp - 2]
+    push ax                                            ; drive_num
+
+    mov ax, STAGE2_SECTOR_COUNT
+    push ax                                            ; count
+
+    mov dword eax, 0x1
+    push dword eax                                     ; lba
 
     xor ax, ax
-    push ax          ; offset = 0
+    push ax                                            ; offset = 0
 
-    ; 07E0:0
+    ; 07E0:0 = 0x00007e00
     mov ax, STAGE2_ENTRY
     shr ax, 4
-    push ax          ; segment = 7E0
+    push ax                                            ; segment = 7E0
 
-    ;uint8_t read_stage2_raw(uint16_t buf_segment, uint16_t buf_offset, 
-    ;                      uint16_t lower_lower_lba, uint16_t upper_lower_lba)
-    call read_stage2_raw
-    add sp, 0x8
+    ; uint8_t read_stage2_raw(uint16_t buf_segment, uint16_t buf_offset, 
+    ;                         uint32_t lba,
+    ;                         uint16_t count, uint16_t drive_num)
+    call read_disk_raw
+    add sp, 0xC
 
 .check_sig:
     mov ax, 0x7E0
@@ -97,97 +135,16 @@ main:
     ERROR VBR_ERROR_NO_SIGNATURE          ; no signature present in stage2
     
 .sig_ok:
-    mov ax, fat32_bpb_SIZE          ; size in byte
-    push ax
-    mov ax, phy_bpb_start           ; start of bpb
-    push ax
-    mov ax, fat32_bpb               ; defined in memory.inc, destination
-    push ax
-    call kmemcpy                    ; copy bpb to memory
-    add sp, 0x6
-
-    mov ax, fat32_ebpb_SIZE          ; 72 bytes of data
-    push ax
-    mov ax, phy_ebpb_start           ; start of ebpb
-    push ax
-    mov ax, fat32_ebpb               ; defined in memory.inc, destination
-    push ax
-    call kmemcpy                     ; copy ebpb to memory
-    add sp, 0x6
-
-    mov si, [partition_offset]
-    mov dl, [bsDriveNumber]
+    mov si, [bp - 4]
+    mov dx, [bp - 2]
     jmp 0:0x7E00
 
-stop:
-    hlt
-    jmp short stop
-
-; Wrapper for AH=0x42 INT13h (Extended Read)
+; ###############
 ;
-; BIOS call details
-; AH = 42h
-; DL = drive number
-; DS:SI -> disk address packet
+; Required BIOS functions
 ;
-; Return:
-; CF clear if successful
-; AH = 00h
-; CF set on error
-; AH = error code
-; disk address packet's block count field set to number of blocks
-; successfully transferred
-;
-;
-; uint8_t read_disk_raw(uint16_t buf_segment, uint16_t buf_offset, 
-;                       uint16_t lower_lower_lba, uint16_t upper_lower_lba)
-read_stage2_raw:
-    __CDECL16_ENTRY
-.func:                        
-    mov ax, 0x10
-    push ax             ; len = 16 bytes
-    xor ax, ax
-    push ax             ; val = 0
-    mov ax, lba_packet
-    push ax             ; dest = lba_packet address
-    call kmemset        
-    add sp, 0x06
-
-    mov byte [lba_packet + LBAPkt_t.size], 0x10
-    mov word [lba_packet + LBAPkt_t.xfer_size], STAGE2_SECTOR_COUNT
-
-    mov ax, [bp + 10]
-    shl eax, 16
-    mov ax, [bp + 8]
-    mov dword [lba_packet + LBAPkt_t.lower_lba], eax
-
-    mov ax, [bp + 6]
-    mov word [lba_packet + LBAPkt_t.offset], ax
-
-    mov ax, [bp + 4]
-    mov word [lba_packet + LBAPkt_t.segment], ax
-
-    mov si, lba_packet
-    mov ah, 0x42
-    mov dl, byte [bsDriveNumber]
-    int 0x13
-    jnc .endf
-
-    ERROR VBR_ERROR_DISK_READ_ERR
-.endf:
-    __CDECL16_EXIT
-    ret
-; Data
-
-; #############
-;
-; Locals
-;
-; #############
-
-; offset from the begining of sector 0 to the active partition.
-partition_offset:
-    dw 0x0000
+; ###############
+%include 'BIOS/func/ext_read.inc'
 
 %assign bytes_remaining (420 - ($ - $$))
 %warning VBR has bytes_remaining bytes remaining for code (MAX: 420 bytes)