int
 main(void)
 {
-
-  printf("Hello from VMXAssist\n");
+    printf("Hello from VMXAssist\n");
 
        if (booting_cpu == 0)
                banner();
 
 #define   HOST_ADDR_SPACE_SIZE          0x00000200
 #define   ACK_IRQ_ON_EXIT               0x00008000
 
+/* Control register exit masks */
+#define   CR0_PE        0x00000001
+#define   CR0_PG        0x80000000
+#define   CR4_VMXE      0x00002000
+
 typedef enum {
     VMCS_GUEST_ES_SELECTOR       = 0x00000800,
     VMCS_GUEST_CS_SELECTOR       = 0x00000802,
 
 
 
-/* VMCS Exit QUALIFICATIONs */
-struct vmcs_io_qual {
-    uint32_t accessSize : 3; // (0: 1 Byte ;; 1: 2 Bytes ;; 3: 4 Bytes)
-    uint32_t dir        : 1; // (0: Out ;; 1: In)
-    uint32_t string     : 1; // (0: not string ;; 1: string)
-    uint32_t REP        : 1; // (0: not REP ;; 1: REP)
-    uint32_t opEnc      : 1; // (0: DX ;; 1: immediate)
-    uint32_t rsvd       : 9; // Set to 0
-    uint32_t port       : 16; // IO Port Number
-} __attribute__((packed));
-
-
-
-struct VMExitDBGQual {
-    uint32_t B0         : 1; // Breakpoint 0 condition met
-    uint32_t B1         : 1; // Breakpoint 1 condition met
-    uint32_t B2         : 1; // Breakpoint 2 condition met
-    uint32_t B3         : 1; // Breakpoint 3 condition met
-    uint32_t rsvd       : 9; // reserved to 0
-    uint32_t BD         : 1; // detected DBG reg access
-    uint32_t BS         : 1; // cause either single instr or taken branch
-} __attribute__((packed));
-
-
-struct VMExitTSQual {
-    uint32_t selector   : 16; // selector of destination TSS 
-    uint32_t rsvd       : 14; // reserved to 0
-    uint32_t src        : 2; // (0: CALL ; 1: IRET ; 2: JMP ; 3: Task gate in IDT)
-} __attribute__((packed));
-
-struct VMExitCRQual {
-    uint32_t crID       : 4; // cr number (0 for CLTS and LMSW) (bit 3 always 0, on 32bit)
-    uint32_t accessType : 2; // (0: MOV to CR ; 1: MOV from CR ; 2: CLTS ; 3: LMSW)
-    uint32_t lmswOpType : 1; // (0: register ; 1: memory)
-    uint32_t rsvd1      : 1; // reserved to 0
-    uint32_t gpr        : 4; // (0:RAX+[CLTS/LMSW], 1:RCX, 2:RDX, 3:RBX, 4:RSP, 5:RBP, 6:RSI, 6:RDI, 8-15:64bit regs)
-    uint32_t rsvd2      : 4; // reserved to 0
-    uint32_t lmswSrc    : 16; // src data for lmsw
-} __attribute__((packed));
-
-struct VMExitMovDRQual {
-    uint32_t regID      : 3; // debug register number
-    uint32_t rsvd1      : 1; // reserved to 0
-    uint32_t dir        : 1; // (0: MOV to DR , 1: MOV from DR)
-    uint32_t rsvd2      : 3; // reserved to 0
-    uint32_t gpr        : 4; // (0:RAX, 1:RCX, 2:RDX, 3:RBX, 4:RSP, 5:RBP, 6:RSI, 6:RDI, 8-15:64bit regs)
-} __attribute__((packed));
-
-/* End Exit Qualifications */
-
 /* Exit Vector Info */
 struct VMExitIntInfo {
     uint32_t nr         : 8; // IRQ number, exception vector, NMI = 2 
 
 /*  End Exit Vector Info */
 
+struct vmx_exception_bitmap {
+    union {
+        uint32_t value;
+    struct {
+        uint_t de          : 1; // (0) divide by zero
+        uint_t db          : 1; // (1) Debug
+        uint_t nmi         : 1; // (2) Non-maskable interrupt
+        uint_t bp          : 1; // (3) Breakpoint
+        uint_t of          : 1; // (4) Overflow
+        uint_t br          : 1; // (5) Bound-Range
+        uint_t ud          : 1; // (6) Invalid-Opcode
+        uint_t nm          : 1; // (7) Device-not-available
+        uint_t df          : 1; // (8) Double Fault
+        uint_t ex9         : 1; 
+        uint_t ts          : 1; // (10) Invalid TSS
+        uint_t np          : 1; // (11) Segment-not-present
+        uint_t ss          : 1; // (12) Stack
+        uint_t gp          : 1; // (13) General Protection Fault
+        uint_t pf          : 1; // (14) Page fault
+        uint_t ex15        : 1;
+        uint_t mf          : 1; // (15) Floating point exception
+        uint_t ac          : 1; // (16) Alignment-check
+        uint_t mc          : 1; // (17) Machine Check
+        uint_t xf          : 1; // (18) SIMD floating-point
+        uint_t ex20        : 1;
+        uint_t ex21        : 1;
+        uint_t ex22        : 1;
+        uint_t ex23        : 1;
+        uint_t ex24        : 1;
+        uint_t ex25        : 1;
+        uint_t ex26        : 1;
+        uint_t ex27        : 1;
+        uint_t ex28        : 1;
+        uint_t ex29        : 1;
+        uint_t sx          : 1; // (30) Security Exception
+        uint_t ex31        : 1;
+    } __attribute__ ((packed));
+    } __attribute__ ((packed));
+} __attribute__((packed));
+
 
 
 
 
 
 // Intel VMX Specific MSRs
 #define VMX_FEATURE_CONTROL_MSR     0x0000003a
-#define VMX_BASIC_MSR          0x00000480
+#define VMX_BASIC_MSR               0x00000480
 #define VMX_PINBASED_CTLS_MSR       0x00000481
 #define VMX_PROCBASED_CTLS_MSR      0x00000482
 #define VMX_EXIT_CTLS_MSR           0x00000483
     uint32_t sec_procbased_ctrls;
     uint32_t exit_ctrls;
     uint32_t entry_ctrls;
-    uint32_t excp_bitmap;
 };
 
 
 
 int v3_is_vmx_capable();
 void v3_init_vmx(struct v3_ctrl_ops* vm_ops);
+int v3_update_vmcs_guest_state(struct guest_info * info);
+int v3_update_vmcs_ctrl_fields(struct guest_info * info);
+int v3_update_vmcs_host_state(struct guest_info * info);
 
 
 
 
     VMEXIT_XSETBV                           = 55
 } vmx_exit_t;
 
+/* VMCS Exit QUALIFICATIONs */
+struct vmexit_io_qual {
+    uint32_t access_size : 3; // (0: 1 Byte ;; 1: 2 Bytes ;; 3: 4 Bytes)
+    uint32_t dir        : 1; // (0: Out ;; 1: In)
+    uint32_t string     : 1; // (0: not string ;; 1: string)
+    uint32_t REP        : 1; // (0: not REP ;; 1: REP)
+    uint32_t op_enc      : 1; // (0: DX ;; 1: immediate)
+    uint32_t rsvd       : 9; // Set to 0
+    uint32_t port       : 16; // IO Port Number
+} __attribute__((packed));
+
+
+
+struct VMExitDBGQual {
+    uint32_t B0         : 1; // Breakpoint 0 condition met
+    uint32_t B1         : 1; // Breakpoint 1 condition met
+    uint32_t B2         : 1; // Breakpoint 2 condition met
+    uint32_t B3         : 1; // Breakpoint 3 condition met
+    uint32_t rsvd       : 9; // reserved to 0
+    uint32_t BD         : 1; // detected DBG reg access
+    uint32_t BS         : 1; // cause either single instr or taken branch
+} __attribute__((packed));
+
+
+struct VMExitTSQual {
+    uint32_t selector   : 16; // selector of destination TSS 
+    uint32_t rsvd       : 14; // reserved to 0
+    uint32_t src        : 2; // (0: CALL ; 1: IRET ; 2: JMP ; 3: Task gate in IDT)
+} __attribute__((packed));
+
+struct vmexit_cr_qual {
+    uint32_t cr_id       : 4; // cr number (0 for CLTS and LMSW) (bit 3 always 0, on 32bit)
+    uint32_t access_type : 2; // (0: MOV to CR ; 1: MOV from CR ; 2: CLTS ; 3: LMSW)
+    uint32_t lmsw_op_type : 1; // (0: register ; 1: memory)
+    uint32_t rsvd1      : 1; // reserved to 0
+    uint32_t gpr        : 4; // (0:RAX+[CLTS/LMSW], 1:RCX, 2:RDX, 3:RBX, 4:RSP, 5:RBP, 6:RSI, 6:RDI, 8-15:64bit regs)
+    uint32_t rsvd2      : 4; // reserved to 0
+    uint32_t lmsw_src    : 16; // src data for lmsw
+} __attribute__((packed));
+
+struct VMExitMovDRQual {
+    uint32_t regID      : 3; // debug register number
+    uint32_t rsvd1      : 1; // reserved to 0
+    uint32_t dir        : 1; // (0: MOV to DR , 1: MOV from DR)
+    uint32_t rsvd2      : 3; // reserved to 0
+    uint32_t gpr        : 4; // (0:RAX, 1:RCX, 2:RDX, 3:RBX, 4:RSP, 5:RBP, 6:RSI, 6:RDI, 8-15:64bit regs)
+} __attribute__((packed));
+
+/* End Exit Qualifications */
+
+
 int v3_handle_vmx_exit(struct v3_gprs * gprs, struct guest_info * info);
 
 #endif
 
     PrintDebug("8259 PIC: Write master port 1 with 0x%x\n",cw);
 
     if (length != 1) {
-       PrintError("8259 PIC: Invalid Write length (wr_Master1)\n");
-       return -1;
+        PrintError("8259 PIC: Invalid Write length (wr_Master1)\n");
+        return -1;
     }
-  
+
     if (IS_ICW1(cw)) {
 
-       PrintDebug("8259 PIC: Setting ICW1 = %x (wr_Master1)\n", cw);
-    
-       state->master_icw1 = cw;
-       state->master_state = ICW2;
+        PrintDebug("8259 PIC: Setting ICW1 = %x (wr_Master1)\n", cw);
 
-    } else if (state->master_state == READY) {
-       if (IS_OCW2(cw)) {
-           // handle the EOI here
-           struct ocw2 * cw2 =  (struct ocw2*)&cw;
-
-           PrintDebug("8259 PIC: Handling OCW2 = %x (wr_Master1)\n", cw);
-      
-           if ((cw2->EOI) && (!cw2->R) && (cw2->SL)) {
-               // specific EOI;
-               state->master_isr &= ~(0x01 << cw2->level);
-           } else if ((cw2->EOI) & (!cw2->R) && (!cw2->SL)) {
-               int i;
-               // Non-specific EOI
-               PrintDebug("8259 PIC: Pre ISR = %x (wr_Master1)\n", state->master_isr);
-               for (i = 0; i < 8; i++) {
-                   if (state->master_isr & (0x01 << i)) {
-                       state->master_isr &= ~(0x01 << i);
-                       break;
-                   }
-               }       
-               PrintDebug("8259 PIC: Post ISR = %x (wr_Master1)\n", state->master_isr);
-           } else {
-               PrintError("8259 PIC: Command not handled, or in error (wr_Master1)\n");
-               return -1;
-           }
+        state->master_icw1 = cw;
+        state->master_state = ICW2;
 
-           state->master_ocw2 = cw;
-       } else if (IS_OCW3(cw)) {
-           PrintDebug("8259 PIC: Handling OCW3 = %x (wr_Master1)\n", cw);
-           state->master_ocw3 = cw;
-       } else {
-           PrintError("8259 PIC: Invalid OCW to PIC (wr_Master1)\n");
-           PrintError("8259 PIC: CW=%x\n", cw);
-           return -1;
-       }
+    } else if (state->master_state == READY) {
+        if (IS_OCW2(cw)) {
+            // handle the EOI here
+            struct ocw2 * cw2 =  (struct ocw2*)&cw;
+
+            PrintDebug("8259 PIC: Handling OCW2 = %x (wr_Master1)\n", cw);
+
+            if ((cw2->EOI) && (!cw2->R) && (cw2->SL)) {
+                // specific EOI;
+                state->master_isr &= ~(0x01 << cw2->level);
+            } else if ((cw2->EOI) & (!cw2->R) && (!cw2->SL)) {
+                int i;
+                // Non-specific EOI
+                PrintDebug("8259 PIC: Pre ISR = %x (wr_Master1)\n", state->master_isr);
+                for (i = 0; i < 8; i++) {
+                    if (state->master_isr & (0x01 << i)) {
+                        state->master_isr &= ~(0x01 << i);
+                        break;
+                    }
+                }      
+                PrintDebug("8259 PIC: Post ISR = %x (wr_Master1)\n", state->master_isr);
+            } else {
+                PrintError("8259 PIC: Command not handled, or in error (wr_Master1)\n");
+                return -1;
+            }
+
+            state->master_ocw2 = cw;
+        } else if (IS_OCW3(cw)) {
+            PrintDebug("8259 PIC: Handling OCW3 = %x (wr_Master1)\n", cw);
+            state->master_ocw3 = cw;
+        } else {
+            PrintError("8259 PIC: Invalid OCW to PIC (wr_Master1)\n");
+            PrintError("8259 PIC: CW=%x\n", cw);
+            return -1;
+        }
     } else {
-       PrintError("8259 PIC: Invalid PIC State (wr_Master1)\n");
-       PrintError("8259 PIC: CW=%x\n", cw);
-       return -1;
+        PrintError("8259 PIC: Invalid PIC State (wr_Master1)\n");
+        PrintError("8259 PIC: CW=%x\n", cw);
+        return -1;
     }
 
     return 1;
     uchar_t cw = *(uchar_t *)src;    
 
     PrintDebug("8259 PIC: Write master port 2 with 0x%x\n",cw);
-  
+
     if (length != 1) {
-       PrintError("8259 PIC: Invalid Write length (wr_Master2)\n");
-       return -1;
+        PrintError("8259 PIC: Invalid Write length (wr_Master2)\n");
+        return -1;
     }
-    
+
     if (state->master_state == ICW2) {
-       struct icw1 * cw1 = (struct icw1 *)&(state->master_icw1);
+        struct icw1 * cw1 = (struct icw1 *)&(state->master_icw1);
 
-       PrintDebug("8259 PIC: Setting ICW2 = %x (wr_Master2)\n", cw);
-       state->master_icw2 = cw;
+        PrintDebug("8259 PIC: Setting ICW2 = %x (wr_Master2)\n", cw);
+        state->master_icw2 = cw;
 
-       if (cw1->sngl == 0) {
-           state->master_state = ICW3;
-       } else if (cw1->ic4 == 1) {
-           state->master_state = ICW4;
-       } else {
-           state->master_state = READY;
-       }
+        if (cw1->sngl == 0) {
+            state->master_state = ICW3;
+        } else if (cw1->ic4 == 1) {
+            state->master_state = ICW4;
+        } else {
+            state->master_state = READY;
+        }
 
     } else if (state->master_state == ICW3) {
-       struct icw1 * cw1 = (struct icw1 *)&(state->master_icw1);
+        struct icw1 * cw1 = (struct icw1 *)&(state->master_icw1);
 
-       PrintDebug("8259 PIC: Setting ICW3 = %x (wr_Master2)\n", cw);
+        PrintDebug("8259 PIC: Setting ICW3 = %x (wr_Master2)\n", cw);
 
-       state->master_icw3 = cw;
+        state->master_icw3 = cw;
 
-       if (cw1->ic4 == 1) {
-           state->master_state = ICW4;
-       } else {
-           state->master_state = READY;
-       }
+        if (cw1->ic4 == 1) {
+            state->master_state = ICW4;
+        } else {
+            state->master_state = READY;
+        }
 
     } else if (state->master_state == ICW4) {
-       PrintDebug("8259 PIC: Setting ICW4 = %x (wr_Master2)\n", cw);
-       state->master_icw4 = cw;
-       state->master_state = READY;
-    } else if (state->master_state == READY) {
-       PrintDebug("8259 PIC: Setting IMR = %x (wr_Master2)\n", cw);
-       state->master_imr = cw;
+        PrintDebug("8259 PIC: Setting ICW4 = %x (wr_Master2)\n", cw);
+        state->master_icw4 = cw;
+        state->master_state = READY;
+    } else if ((state->master_state == ICW1) || (state->master_state == READY)) {
+        PrintDebug("8259 PIC: Setting IMR = %x (wr_Master2)\n", cw);
+        state->master_imr = cw;
     } else {
-       // error
-       PrintError("8259 PIC: Invalid master PIC State (wr_Master2)\n");
-       return -1;
+        // error
+        PrintError("8259 PIC: Invalid master PIC State (wr_Master2) (state=%d)\n", 
+                state->master_state);
+        return -1;
     }
 
     return 1;
     PrintDebug("8259 PIC: Write slave port 2 with 0x%x\n",cw);
 
     if (length != 1) {
-       PrintError("8259 PIC: Invalid write length (wr_Slave2)\n");
-       return -1;
+        PrintError("8259 PIC: Invalid write length (wr_Slave2)\n");
+        return -1;
     }
 
     if (state->slave_state == ICW2) {
-       struct icw1 * cw1 =  (struct icw1 *)&(state->master_icw1);
+        struct icw1 * cw1 =  (struct icw1 *)&(state->master_icw1);
 
-       PrintDebug("8259 PIC: Setting ICW2 = %x (wr_Slave2)\n", cw);
+        PrintDebug("8259 PIC: Setting ICW2 = %x (wr_Slave2)\n", cw);
 
-       state->slave_icw2 = cw;
+        state->slave_icw2 = cw;
 
-       if (cw1->sngl == 0) {
-           state->slave_state = ICW3;
-       } else if (cw1->ic4 == 1) {
-           state->slave_state = ICW4;
-       } else {
-           state->slave_state = READY;
-       }
+        if (cw1->sngl == 0) {
+            state->slave_state = ICW3;
+        } else if (cw1->ic4 == 1) {
+            state->slave_state = ICW4;
+        } else {
+            state->slave_state = READY;
+        }
 
     } else if (state->slave_state == ICW3) {
-       struct icw1 * cw1 =  (struct icw1 *)&(state->master_icw1);
+        struct icw1 * cw1 =  (struct icw1 *)&(state->master_icw1);
 
-       PrintDebug("8259 PIC: Setting ICW3 = %x (wr_Slave2)\n", cw);
+        PrintDebug("8259 PIC: Setting ICW3 = %x (wr_Slave2)\n", cw);
 
-       state->slave_icw3 = cw;
+        state->slave_icw3 = cw;
 
-       if (cw1->ic4 == 1) {
-           state->slave_state = ICW4;
-       } else {
-           state->slave_state = READY;
-       }
+        if (cw1->ic4 == 1) {
+            state->slave_state = ICW4;
+        } else {
+            state->slave_state = READY;
+        }
 
     } else if (state->slave_state == ICW4) {
-       PrintDebug("8259 PIC: Setting ICW4 = %x (wr_Slave2)\n", cw);
-       state->slave_icw4 = cw;
-       state->slave_state = READY;
-    } else if (state->slave_state == READY) {
-       PrintDebug("8259 PIC: Setting IMR = %x (wr_Slave2)\n", cw);
-       state->slave_imr = cw;
+        PrintDebug("8259 PIC: Setting ICW4 = %x (wr_Slave2)\n", cw);
+        state->slave_icw4 = cw;
+        state->slave_state = READY;
+    } else if ((state->slave_state == ICW1) || (state->slave_state == READY)) {
+        PrintDebug("8259 PIC: Setting IMR = %x (wr_Slave2)\n", cw);
+        state->slave_imr = cw;
     } else {
-       PrintError("8259 PIC: Invalid State at write (wr_Slave2)\n");
-       return -1;
+        PrintError("8259 PIC: Invalid State at write (wr_Slave2)\n");
+        return -1;
     }
 
     return 1;
 
 #include <palacios/vmm.h>
 #include <palacios/vmx_lowlevel.h>
 #include <palacios/vmm_lowlevel.h>
-#include <palacios/vmm_config.h>
 #include <palacios/vmm_ctrl_regs.h>
+#include <palacios/vmm_config.h>
 #include <palacios/vm_guest_mem.h>
 #include <palacios/vmm_direct_paging.h>
 #include <palacios/vmx_io.h>
     access->granularity = v3_seg->granularity;
 }
 
-static int update_vmcs_ctrl_fields(struct guest_info * info) {
+int v3_update_vmcs_ctrl_fields(struct guest_info * info) {
     int vmx_ret = 0;
     struct vmx_data * arch_data = (struct vmx_data *)(info->vmm_data);
 
 
     vmx_ret |= check_vmcs_write(VMCS_EXIT_CTRLS, arch_data->exit_ctrls);
     vmx_ret |= check_vmcs_write(VMCS_ENTRY_CTRLS, arch_data->entry_ctrls);
-    vmx_ret |= check_vmcs_write(VMCS_EXCP_BITMAP, arch_data->excp_bitmap);
 
     return vmx_ret;
 }
 
-static int update_vmcs_host_state(struct guest_info * info) {
+int v3_update_vmcs_host_state(struct guest_info * info) {
     int vmx_ret = 0;
     addr_t tmp;
     struct vmx_data * arch_data = (struct vmx_data *)(info->vmm_data);
 }
 
 
-static int inline update_vmcs_guest_state(struct guest_info * info)
+int v3_update_vmcs_guest_state(struct guest_info * info)
 {
-    struct v3_msr tmp_msr;
     int vmx_ret = 0;
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_RIP, info->rip);
     vmx_ret |= check_vmcs_write(VMCS_GUEST_CR4, info->ctrl_regs.cr4);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_RFLAGS, info->ctrl_regs.rflags);
-#define DEBUGCTL_MSR 0x1d9
-
-    v3_get_msr(DEBUGCTL_MSR, &(tmp_msr.hi), &(tmp_msr.lo));
-    vmx_ret |= check_vmcs_write(VMCS_GUEST_DBG_CTL, tmp_msr.value);
-
-    vmx_ret |= check_vmcs_write(VMCS_GUEST_DR7, 0x400);
 
-    vmx_ret |= check_vmcs_write(VMCS_LINK_PTR, 0xffffffffffffffff);
 
 
     /*** Write VMCS Segments ***/
     vmx_ret |= check_vmcs_write(VMCS_GUEST_CS_ACCESS, access.value);
 
     /* SS Segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.ss), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_SS_BASE, info->segments.ss.base);
     vmx_ret |= check_vmcs_write(VMCS_GUEST_SS_ACCESS, access.value);
 
     /* DS Segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.ds), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_DS_BASE, info->segments.ds.base);
 
 
     /* ES Segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.es), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_ES_BASE, info->segments.es.base);
     vmx_ret |= check_vmcs_write(VMCS_GUEST_ES_ACCESS, access.value);
 
     /* FS Segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.fs), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_FS_BASE, info->segments.fs.base);
     vmx_ret |= check_vmcs_write(VMCS_GUEST_FS_ACCESS, access.value);
 
     /* GS Segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.gs), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_GS_BASE, info->segments.gs.base);
     vmx_ret |= check_vmcs_write(VMCS_GUEST_GS_ACCESS, access.value);
 
     /* LDTR segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.ldtr), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_LDTR_BASE, info->segments.ldtr.base);
     vmx_ret |= check_vmcs_write(VMCS_GUEST_LDTR_ACCESS, access.value);
 
     /* TR Segment */
+    memset(&access, 0, sizeof(access));
     translate_segment_access(&(info->segments.tr), &access);
 
     vmx_ret |= check_vmcs_write(VMCS_GUEST_TR_BASE, info->segments.tr.base);
 }
 
 #if 0
-static void setup_v8086_mode_for_boot(struct guest_info * vm_info)
-{
 
-    ((struct vmx_data *)vm_info->vmm_data)->state = VMXASSIST_V8086_BIOS;
-    struct rflags * flags = (struct rflags *)&(vm_info->ctrl_regs.rflags);
-    flags->rsvd1 = 1;
-    flags->vm = 1;
-    flags->iopl = 3;
-
-#define GUEST_CR0_MASK 0x80000021
-#define GUEST_CR4_MASK 0x00002000
-    vm_info->ctrl_regs.cr0 = GUEST_CR0_MASK;
-    vm_info->ctrl_regs.cr4 = GUEST_CR4_MASK;
-   
-    vm_info->rip = 0xd0000;
-    vm_info->vm_regs.rsp = 0x80000;
-
-    vm_info->segments.cs.selector = 0xf000;
-    vm_info->segments.cs.base = 0xf000 << 4;
-    vm_info->segments.cs.limit = 0xffff;
-    vm_info->segments.cs.type = 3;
-    vm_info->segments.cs.system = 1;
-    vm_info->segments.cs.dpl = 3;
-    vm_info->segments.cs.present = 1;
-    vm_info->segments.cs.granularity = 0;
-
-    int i = 0;
-    struct v3_segment * seg_ptr = (struct v3_segment *)&(vm_info->segments);
-
-    /* Set values for selectors ds through ss */
-    for(i = 1; i < 6 ; i++) {
-        seg_ptr[i].selector = 0x0000;
-        seg_ptr[i].base = 0x00000;
-        seg_ptr[i].limit = 0xffff;
-           }
-
-    for(i = 6; i < 10; i++) {
-        seg_ptr[i].base = 0x0;
-        seg_ptr[i].limit = 0xffff;
-    }
-
-    vm_info->segments.ldtr.selector = 0x0;
-    vm_info->segments.ldtr.type = 2;
-    vm_info->segments.ldtr.system = 0;
-    vm_info->segments.ldtr.present = 1;
-    vm_info->segments.ldtr.granularity = 0;
-
-    vm_info->segments.tr.selector = 0x0;
-    vm_info->segments.tr.type = 3;
-    vm_info->segments.tr.system = 0;
-    vm_info->segments.tr.present = 1;
-    vm_info->segments.tr.granularity = 0;
-}
 #endif
 
 #if 0
     vmx_data->pri_procbased_ctrls = tmp_msr.lo;
 
     v3_get_msr(VMX_EXIT_CTLS_MSR, &(tmp_msr.hi), &(tmp_msr.lo));
-    vmx_data->exit_ctrls = tmp_msr.lo | HOST_ADDR_SPACE_SIZE;
+    vmx_data->exit_ctrls = tmp_msr.lo ;
 
     v3_get_msr(VMX_ENTRY_CTLS_MSR, &(tmp_msr.hi), &(tmp_msr.lo));
     vmx_data->entry_ctrls = tmp_msr.lo;
 
-    vmx_data->excp_bitmap = 0xffffffff;
-
+    struct vmx_exception_bitmap excp_bmap;
+    excp_bmap.value = 0xffffffff;
+    vmx_ret |= check_vmcs_write(VMCS_EXCP_BITMAP, excp_bmap.value);
 
 
     /******* Setup VMXAssist guest state ***********/
             return -1;
         }
 
-        info->shdw_pg_state.guest_cr0 = 0x10LL;
+        info->shdw_pg_state.guest_cr0 = CR0_PE;
         PrintDebug("Created\n");
 
-        vmx_ret |= check_vmcs_write(VMCS_CR0_MASK, 0xffffffffffffffffLL);
-        vmx_ret |= check_vmcs_write(VMCS_CR4_MASK, 0xffffffffffffffffLL);
+        vmx_ret |= check_vmcs_write(VMCS_CR0_MASK, (CR0_PE | CR0_PG) );
+        vmx_ret |= check_vmcs_write(VMCS_CR0_READ_SHDW, info->shdw_pg_state.guest_cr0);
+        vmx_ret |= check_vmcs_write(VMCS_CR4_MASK, CR4_VMXE);
 
         info->ctrl_regs.cr3 = info->direct_map_pt;
 
+        // vmx_data->pinbased_ctrls |= NMI_EXIT;
+
         /* Add unconditional I/O and CR exits */
-        vmx_data->pri_procbased_ctrls |= UNCOND_IO_EXIT  |
-                                         CR3_LOAD_EXIT   |
-                                         CR3_STORE_EXIT;  
+        vmx_data->pri_procbased_ctrls |= UNCOND_IO_EXIT  
+                                        | CR3_LOAD_EXIT  
+                                        | CR3_STORE_EXIT;
  
+        vmx_data->exit_ctrls |= HOST_ADDR_SPACE_SIZE;
     }
 
     struct v3_segment * seg_reg = (struct v3_segment *)&(info->segments);
     uint64_t  gdt[] __attribute__ ((aligned(32))) = {
         0x0000000000000000ULL,         /* 0x00: reserved */
         0x0000830000000000ULL,         /* 0x08: 32-bit TSS */
-        //     0x0000890000000000ULL,          /* 0x08: 32-bit TSS */
+       //0x0000890000000000ULL,                /* 0x08: 32-bit TSS */
         0x00CF9b000000FFFFULL,         /* 0x10: CS 32-bit */
         0x00CF93000000FFFFULL,         /* 0x18: DS 32-bit */
         0x000082000000FFFFULL,         /* 0x20: LDTR 32-bit */
     info->segments.tr.selector = 0x08;
     info->segments.tr.base = vmxassist_tss;
 
-    // info->segments.tr.type = 0x9; 
+    //info->segments.tr.type = 0x9; 
     info->segments.tr.type = 0x3;
     info->segments.tr.system = 0;
     info->segments.tr.present = 1;
     memcpy((void*)vmxassist_dst, v3_vmxassist_start, v3_vmxassist_end - v3_vmxassist_start);
     
     /*** Write all the info to the VMCS ***/
-    if(update_vmcs_ctrl_fields(info)) {
+
+#define DEBUGCTL_MSR 0x1d9
+    v3_get_msr(DEBUGCTL_MSR, &(tmp_msr.hi), &(tmp_msr.lo));
+    vmx_ret |= check_vmcs_write(VMCS_GUEST_DBG_CTL, tmp_msr.value);
+
+    vmx_ret |= check_vmcs_write(VMCS_GUEST_DR7, 0x400);
+
+    vmx_ret |= check_vmcs_write(VMCS_LINK_PTR, 0xffffffffffffffff);
+    
+    if(v3_update_vmcs_ctrl_fields(info)) {
         PrintError("Could not write control fields!\n");
         return -1;
     }
     
-    if(update_vmcs_host_state(info)) {
+    if(v3_update_vmcs_host_state(info)) {
         PrintError("Could not write host state\n");
         return -1;
     }
 
 
-    if(update_vmcs_guest_state(info) != VMX_SUCCESS) {
+    if(v3_update_vmcs_guest_state(info) != VMX_SUCCESS) {
         PrintError("Writing guest state failed!\n");
         return -1;
     }
 
     v3_print_vmcs();
 
+    vmx_data->state = VMXASSIST_STARTUP;
 
     v3_post_config_guest(info, config_ptr);
 
 
 #include <palacios/vmcs.h>
 #include <palacios/vmx_lowlevel.h>
 #include <palacios/vmx_io.h>
+#include <palacios/vmx.h>
+#include <palacios/vmm_ctrl_regs.h>
 
 
 static int inline check_vmcs_write(vmcs_field_t field, addr_t val)
     return 0;
 }
 
+static void inline translate_access_to_v3_seg(struct vmcs_segment_access * access, 
+        struct v3_segment * v3_seg)
+{
+    v3_seg->type = access->type;
+    v3_seg->system = access->desc_type;
+    v3_seg->dpl = access->dpl;
+    v3_seg->present = access->present;
+    v3_seg->avail = access->avail;
+    v3_seg->long_mode = access->long_mode;
+    v3_seg->db = access->db;
+    v3_seg->granularity = access->granularity;
+}
+
+static void load_vmcs_guest_state(struct guest_info * info)
+{
+    check_vmcs_read(VMCS_GUEST_RIP, &(info->rip));
+    check_vmcs_read(VMCS_GUEST_RSP, &(info->vm_regs.rsp));
+    check_vmcs_read(VMCS_GUEST_CR0, &(info->ctrl_regs.cr0));
+    check_vmcs_read(VMCS_GUEST_CR3, &(info->ctrl_regs.cr3));
+    check_vmcs_read(VMCS_GUEST_CR4, &(info->ctrl_regs.cr4));
+
+    struct vmcs_segment_access access;
+
+    memset(&access, 0, sizeof(access));
+
+    /* CS Segment */
+    check_vmcs_read(VMCS_GUEST_CS_BASE, &(info->segments.cs.base));
+    check_vmcs_read(VMCS_GUEST_CS_SELECTOR, &(info->segments.cs.selector));
+    check_vmcs_read(VMCS_GUEST_CS_LIMIT, &(info->segments.cs.limit));
+    check_vmcs_read(VMCS_GUEST_CS_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.cs));
+
+    /* SS Segment */
+    check_vmcs_read(VMCS_GUEST_SS_BASE, &(info->segments.ss.base));
+    check_vmcs_read(VMCS_GUEST_SS_SELECTOR, &(info->segments.ss.selector));
+    check_vmcs_read(VMCS_GUEST_SS_LIMIT, &(info->segments.ss.limit));
+    check_vmcs_read(VMCS_GUEST_SS_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.ss));
+
+    /* DS Segment */
+    check_vmcs_read(VMCS_GUEST_DS_BASE, &(info->segments.ds.base));
+    check_vmcs_read(VMCS_GUEST_DS_SELECTOR, &(info->segments.ds.selector));
+    check_vmcs_read(VMCS_GUEST_DS_LIMIT, &(info->segments.ds.limit));
+    check_vmcs_read(VMCS_GUEST_DS_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.ds));
+
+    /* ES Segment */
+    check_vmcs_read(VMCS_GUEST_ES_BASE, &(info->segments.es.base));
+    check_vmcs_read(VMCS_GUEST_ES_SELECTOR, &(info->segments.es.selector));
+    check_vmcs_read(VMCS_GUEST_ES_LIMIT, &(info->segments.es.limit));
+    check_vmcs_read(VMCS_GUEST_ES_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.es));
+
+    /* FS Segment */
+    check_vmcs_read(VMCS_GUEST_FS_BASE, &(info->segments.fs.base));
+    check_vmcs_read(VMCS_GUEST_FS_SELECTOR, &(info->segments.fs.selector));
+    check_vmcs_read(VMCS_GUEST_FS_LIMIT, &(info->segments.fs.limit));
+    check_vmcs_read(VMCS_GUEST_FS_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.fs));
+
+
+    /* GS Segment */
+    check_vmcs_read(VMCS_GUEST_GS_BASE, &(info->segments.gs.base));
+    check_vmcs_read(VMCS_GUEST_GS_SELECTOR, &(info->segments.gs.selector));
+    check_vmcs_read(VMCS_GUEST_GS_LIMIT, &(info->segments.gs.limit));
+    check_vmcs_read(VMCS_GUEST_GS_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.gs));
+
+    /* LDTR Segment */
+    check_vmcs_read(VMCS_GUEST_LDTR_BASE, &(info->segments.ldtr.base));
+    check_vmcs_read(VMCS_GUEST_LDTR_SELECTOR, &(info->segments.ldtr.selector));
+    check_vmcs_read(VMCS_GUEST_LDTR_LIMIT, &(info->segments.ldtr.limit));
+    check_vmcs_read(VMCS_GUEST_LDTR_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.ldtr));
+
+    /* TR Segment */
+    check_vmcs_read(VMCS_GUEST_TR_BASE, &(info->segments.tr.base));
+    check_vmcs_read(VMCS_GUEST_TR_SELECTOR, &(info->segments.tr.selector));
+    check_vmcs_read(VMCS_GUEST_TR_LIMIT, &(info->segments.tr.limit));
+    check_vmcs_read(VMCS_GUEST_TR_ACCESS, &(access.value));
+
+    translate_access_to_v3_seg(&access, &(info->segments.tr));
+
+    /* GDTR Segment */
+    check_vmcs_read(VMCS_GUEST_GDTR_BASE, &(info->segments.gdtr.base));
+    check_vmcs_read(VMCS_GUEST_GDTR_LIMIT, &(info->segments.gdtr.limit));
+    
+    /* IDTR Segment */
+    check_vmcs_read(VMCS_GUEST_IDTR_BASE, &(info->segments.idtr.base));
+    check_vmcs_read(VMCS_GUEST_IDTR_LIMIT, &(info->segments.idtr.limit));
+}
+
+
+static void setup_v8086_mode_for_boot(struct guest_info * info)
+{
+
+    ((struct vmx_data *)info->vmm_data)->state = VMXASSIST_V8086_BIOS;
+    struct rflags * flags = (struct rflags *)&(info->ctrl_regs.rflags);
+    flags->rsvd1 = 1;
+    flags->vm = 1;
+    flags->iopl = 3;
+
+    info->rip = 0xfff0;
+    //info->vm_regs.rsp = 0x0;
+   
+    /* Zero the segment registers */
+    memset(&(info->segments), 0, sizeof(struct v3_segment)*6);
+
+
+    info->segments.cs.selector = 0xf000;
+    info->segments.cs.base = 0xf000 << 4;
+    info->segments.cs.limit = 0xffff;
+    info->segments.cs.type = 3;
+    info->segments.cs.system = 1;
+    info->segments.cs.dpl = 3;
+    info->segments.cs.present = 1;
+    info->segments.cs.granularity = 0;
+
+    int i;
+    
+    /* Set values for selectors ds through ss */
+    struct v3_segment * seg_ptr = (struct v3_segment *)&(info->segments);
+    for(i = 1; i < 6 ; i++) {
+        seg_ptr[i].selector = 0x0000;
+        seg_ptr[i].base = 0x00000;
+        seg_ptr[i].limit = 0xffff;
+        seg_ptr[i].type = 3;
+        seg_ptr[i].system = 1;
+        seg_ptr[i].dpl = 3;
+        seg_ptr[i].present = 1;
+        seg_ptr[i].granularity = 0;
+    }
+
+    PrintDebug("END INFO!\n");
+#if 0
+    for(i = 6; i < 10; i++) {
+        seg_ptr[i].base = 0x0;
+        seg_ptr[i].limit = 0xffff;
+    }
+
+    info->segments.ldtr.type = 2;
+    info->segments.ldtr.system = 0;
+    info->segments.ldtr.present = 1;
+    info->segments.ldtr.granularity = 0;
+
+    info->segments.tr.type = 3;
+    info->segments.tr.system = 0;
+    info->segments.tr.present = 1;
+    info->segments.tr.granularity = 0;
+#endif
+}
+
+static int inline handle_cr_access(struct guest_info * info, ulong_t exit_qual)
+{
+    struct vmexit_cr_qual * cr_qual = (struct vmexit_cr_qual *)&exit_qual;
+
+    if(cr_qual->access_type < 2) {
+        ulong_t reg = 0;
+        switch(cr_qual->gpr) {
+            case 0:
+                reg = info->vm_regs.rax;
+                break;
+            case 1:
+                reg = info->vm_regs.rcx;
+                break;
+            case 2:
+                reg = info->vm_regs.rdx;
+                break;
+            case 3:
+                reg = info->vm_regs.rbx;
+                break;
+            case 4:
+                reg = info->vm_regs.rsp;
+                break;
+            case 5:
+                reg = info->vm_regs.rbp;
+                break;
+            case 6:
+                reg = info->vm_regs.rsi;
+                break;
+            case 7:
+                reg = info->vm_regs.rdi;
+                break;
+            case 8:
+                reg = info->vm_regs.r8;
+                break;
+            case 9:
+                reg = info->vm_regs.r9;
+                break;
+            case 10:
+                reg = info->vm_regs.r10;
+                break;
+            case 11:
+                reg = info->vm_regs.r11;
+                break;
+            case 12:
+                reg = info->vm_regs.r11;
+                break;
+            case 13:
+                reg = info->vm_regs.r13;
+                break;
+            case 14:
+                reg = info->vm_regs.r14;
+                break;
+            case 15:
+                reg = info->vm_regs.r15;
+                break;
+        }
+        PrintDebug("RAX: %p\n", (void *)info->vm_regs.rax);
+
+        if(cr_qual->cr_id == 0
+                && (~reg & CR0_PE)
+                && ((struct vmx_data*)info->vmm_data)->state == VMXASSIST_STARTUP) {
+            setup_v8086_mode_for_boot(info);
+            info->shdw_pg_state.guest_cr0 = 0x0;
+            v3_update_vmcs_guest_state(info);
+            return 0;
+        }
+    }
+    PrintError("Unhandled CR access\n");
+    return -1;
+}
+
+
 int v3_handle_vmx_exit(struct v3_gprs * gprs, struct guest_info * info)
 {
     uint32_t exit_reason;
     check_vmcs_read(VMCS_EXIT_REASON, &exit_reason);
     check_vmcs_read(VMCS_EXIT_QUAL, &exit_qual);
 
-    PrintDebug("VMX Exit taken, id-qual: %d-%ld\n", exit_reason, exit_qual);
+    PrintDebug("VMX Exit taken, id-qual: %u-%lu\n", exit_reason, exit_qual);
 
     /* Update guest state */
-    check_vmcs_read(VMCS_GUEST_RIP, &(info->rip));
-    check_vmcs_read(VMCS_GUEST_RSP, &(info->vm_regs.rsp));
-    check_vmcs_read(VMCS_GUEST_CR0, &(info->ctrl_regs.cr0));
-    check_vmcs_read(VMCS_GUEST_CR3, &(info->ctrl_regs.cr3));
-    check_vmcs_read(VMCS_GUEST_CR4, &(info->ctrl_regs.cr4));
-
-    // read out segments
-
+    load_vmcs_guest_state(info);
+  
     switch(exit_reason)
     {
         case VMEXIT_INFO_EXCEPTION_OR_NMI:
                     PrintError("Page fault in unimplemented paging mode\n");
                     return -1;
                 }
+            } else {
+                PrintDebug("Unknown exception: 0x%x\n", (uint8_t)int_info);
+                v3_print_GPRs(info);
+                return -1;
             }
             break;
         }
 
         case VMEXIT_IO_INSTR: 
         {
-            struct vmcs_io_qual * io_qual = (struct vmcs_io_qual *)&exit_qual;
+            struct vmexit_io_qual * io_qual = (struct vmexit_io_qual *)&exit_qual;
 
             if(io_qual->dir == 0) {
                 if(io_qual->string) {
             break;
         }
 
+        case VMEXIT_CR_REG_ACCESSES:
+            if(handle_cr_access(info,exit_qual) != 0)
+                return -1;
+            break;
+
         default:
             PrintError("Unhandled VMEXIT\n");
             return -1;
     check_vmcs_write(VMCS_GUEST_RIP, info->rip);
     check_vmcs_write(VMCS_GUEST_RSP, info->vm_regs.rsp);
 
-    PrintDebug("Executing VMRESUME\n");
+    check_vmcs_write(VMCS_CR0_READ_SHDW, info->shdw_pg_state.guest_cr0);
+
     return 0;
 }
 
 #include <palacios/vmcs.h>
 #include <palacios/vmx_lowlevel.h>
 #include <palacios/vmm.h>
+#include <palacios/vmx_handler.h>
 
 /* Same as SVM */
 static int update_map(struct guest_info * info, uint16_t port, int hook_read, int hook_write)
 
 int v3_handle_vmx_io_in(struct guest_info * info)
 {
-    PrintDebug("IN not implemented\n");
-    return -1;
+    ulong_t exit_qual;
+
+    vmcs_read(VMCS_EXIT_QUAL, &exit_qual);
+
+    struct vmexit_io_qual * io_qual = (struct vmexit_io_qual *)&exit_qual;
+
+    struct v3_io_hook * hook = v3_get_io_hook(info,io_qual->port);
+    int read_size = 0;
+
+    if(hook == NULL) {
+        PrintError("Hook not present for IN on port %x\n", io_qual->port);
+        return -1;
+    }
+
+    read_size = 1<<(io_qual->access_size);
+
+    PrintDebug("IN of %d bytes on port %d (0x%x)\n", read_size, io_qual->port, io_qual->port);
+
+    if(hook->read(io_qual->port, &(info->vm_regs.rax), read_size, hook->priv_data) != read_size) {
+        PrintError("Read failure for IN on port %x\n", io_qual->port);
+        return -1;
+    }
+
+    uint32_t instr_length = 0;
+
+    if(vmcs_read(VMCS_EXIT_INSTR_LEN, &instr_length) != VMX_SUCCESS) {
+        PrintError("Could not read instruction length\n");
+        return -1;
+    }
+
+    info->rip += instr_length;
+
+    return 0;
 }
 
 int v3_handle_vmx_io_ins(struct guest_info * info)
 
     vmcs_read(VMCS_EXIT_QUAL, &exit_qual);
 
-    struct vmcs_io_qual * io_qual = (struct vmcs_io_qual *)&exit_qual;
+    struct vmexit_io_qual * io_qual = (struct vmexit_io_qual *)&exit_qual;
 
     struct v3_io_hook * hook = v3_get_io_hook(info, io_qual->port);
 
         return -1;
     }
 
-    int write_size = 1<<(io_qual->accessSize);
+    int write_size = 1<<(io_qual->access_size);
     
     PrintDebug("OUT of %d bytes on port %d (0x%x)\n", write_size, io_qual->port, io_qual->port);
 
+
     if(hook->write(io_qual->port, &(info->vm_regs.rax), write_size, hook->priv_data) != write_size) {
         PrintError("Write failure for out on port %x\n",io_qual->port);
         return -1;
     }
 
-    uint32_t instr_length;
+    uint32_t instr_length = 0;
 
-    vmcs_read(VMCS_EXIT_INSTR_LEN, &instr_length);
+    if(vmcs_read(VMCS_EXIT_INSTR_LEN, &instr_length) != VMX_SUCCESS) {
+        PrintError("Could not read instruction length\n");
+        return -1;
+    } 
 
     info->rip += instr_length;
 
 
     vmcs_read(VMCS_EXIT_QUAL, &exit_qual);
 
-    struct vmcs_io_qual * io_qual = (struct vmcs_io_qual *)&exit_qual;
+    struct vmexit_io_qual * io_qual = (struct vmexit_io_qual *)&exit_qual;
 
     PrintDebug("OUTS on port %d, (0x%x)\n", io_qual->port, io_qual->port);
     return -1;
 
        movq    %rdx, 40(%rax);         \
        movq    %rcx, 48(%rax);         \
        pushq   %rbx;                   \
-       movq    16(%rsp), %rbx;         \
+       movq    8(%rsp), %rbx;          \
        movq    %rbx, 56(%rax);         \
-       popq    %rbx;                   \
+       popq    %rbx;                       \
                                        \
        movq    %r8, 64(%rax);          \
        movq    %r9, 72(%rax);          \
        
 
 #define restore_registers(location) \
-       push    %rax;                   \
        mov     location, %rax;         \
        mov     (%rax), %rdi;           \
        mov     8(%rax), %rsi;          \
        mov     16(%rax), %rbp;         \
        mov     32(%rax), %rbx;         \
        mov     40(%rax), %rdx;         \
-       mov     48(%rax), %rcx;         \
+       mov 48(%rax), %rcx;             \
                                        \
        mov     64(%rax), %r8;          \
        mov     72(%rax), %r9;          \
        mov     104(%rax), %r13;        \
        mov     112(%rax), %r14;        \
        mov     120(%rax), %r15;        \
-       pop     %rax;
+    pushq %rbx;              \
+    movq 56(%rax), %rbx;     \
+    movq %rbx, %rax;         \
+    popq %rbx;
+