Setting up a VESA video mode

lboot.S

@@ -116,8 +116,13 @@ _start:     cli
             call        read_file
             mov         %ebx, bb_ird_ptr
 
+            mov         $msg_vbe, %si
+            call        print
+            call        vbe_setup
+
             mov         $msg_long, %si
             call        print
+            jmp         hang // REMOVE ME
             call        long
 
             jmp         hang
@@ -498,6 +503,112 @@ dump:       push        %eax
             pop         %eax
             ret
 
+vbe_setup:
+            push        %ecx
+            push        %esi
+            push        %bp
+            mov         %sp, %bp
+
+            call        vbe_getinfo
+
+            mov         vbe_info+14, %si
+            mov         vbe_info+16, %cx
+            mov         %cx, %fs
+
+_vbenext:   mov         %fs:(%si), %cx
+            add         $2, %si
+            cmp         $0xFFFF, %cx
+            je          _vbedone
+
+            call        vbe_getmode
+
+            testw       $0x80, tx_buf
+            jz          _vbenext
+
+            mov         %cx, %bx
+            and         $0x0FFF, %bx
+            or          $0x4000, %bx
+            call        vbe_setmode
+
+            jmp         _vbedone
+
+_vbedone:   
+            leave
+            pop         %esi
+            pop         %ecx
+            ret
+
+vbe_getinfo:
+            push        %eax
+            push        %edi
+            push        %bp
+            mov         %sp, %bp
+
+            mov         $0x4F00, %ax
+            mov         $vbe_info, %di
+            int         $0x10
+
+            cmp         $0x4F, %ax
+            je          1f
+            mov         $msg_vbeerr, %si
+            call        print
+            jmp         hang
+1:
+
+            cmpl        $0x41534556, vbe_info // "VESA"
+            je          1f
+            mov         $msg_vbeerr, %si
+            call        print
+            jmp         hang
+
+1:          leave
+            pop         %edi
+            pop         %eax
+            ret
+
+vbe_getmode:
+            push        %eax
+            push        %edi
+            push        %bp
+            mov         %sp, %bp
+
+            mov         $0x4F01, %ax
+            mov         $tx_buf, %di
+            int         $0x10
+
+            cmp         $0x4F, %ax
+            je          1f
+            mov         $msg_vbeerr, %si
+            call        print
+            jmp         hang
+
+1:          leave
+            pop         %edi
+            pop         %eax
+            ret
+
+vbe_setmode:
+            push        %eax
+            push        %bp
+            mov         %sp, %bp
+
+            mov         $0x4F02, %ax
+            int         $0x10
+
+            cmp         $0x4F, %ax
+            je          1f
+            mov         $msg_vbeerr, %si
+            call        print
+            jmp         hang
+
+1:          leave
+            pop         %eax
+            ret
+
+vbe_info:
+            .ascii      "VBE2"
+            .skip       512 - 4
+
             // long: Enter long mode
 long:       
             // Enable PAE
@@ -594,9 +705,11 @@ msg_getmap: .asciz      " * Memory Map\r\n"
 msg_mkheap: .asciz      " * Making Space\r\n"
 msg_paging: .asciz      " * Paging\r\n"
 msg_read:   .asciz      " * Retrieving\r\n"
+msg_vbe:    .asciz      " * Framebuffer\r\n"
 msg_long:   .asciz      " * Long Mode\r\n"
 msg_pserr:  .asciz      "panic: Missing !PXE structure.\r\n"
 msg_pcerr:  .asciz      "panic: PXE call failed.\r\n"
+msg_vbeerr: .asciz      "panic: VBE call failed.\r\n"
 msg_memerr: .asciz      "panic: Out of heap space.\r\n"
 msg_topen:  .asciz      "tftp_open\r\n"
 msg_tclose: .asciz      "tftp_close\r\n"