[NTVDM]
[reactos.git] / subsystems / ntvdm / bios.c
index 6a99625..cc6cf0f 100644 (file)
 
 #define NDEBUG
 
-#include "bios.h"
 #include "emulator.h"
+#include "bios.h"
+
+#include "vga.h"
 #include "pic.h"
 #include "ps2.h"
 #include "timer.h"
 
+#include "int32.h"
+#include "registers.h"
+
+/* MACROS *********************************************************************/
+
+//
+// These macros are defined for ease-of-use of some VGA I/O ports
+// whose addresses depend whether we are in Monochrome or Colour mode.
+//
+#define VGA_INSTAT1_READ    Bda->CrtBasePort + 6    // VGA_INSTAT1_READ_MONO or VGA_INSTAT1_READ_COLOR
+#define VGA_CRTC_INDEX      Bda->CrtBasePort        // VGA_CRTC_INDEX_MONO   or VGA_CRTC_INDEX_COLOR
+#define VGA_CRTC_DATA       Bda->CrtBasePort + 1    // VGA_CRTC_DATA_MONO    or VGA_CRTC_DATA_COLOR
+
 /* PRIVATE VARIABLES **********************************************************/
 
-static PBIOS_DATA_AREA Bda;
+PBIOS_DATA_AREA Bda;
 static BYTE BiosKeyboardMap[256];
-static HANDLE BiosConsoleInput = INVALID_HANDLE_VALUE;
+static HANDLE BiosConsoleInput  = INVALID_HANDLE_VALUE;
 static HANDLE BiosConsoleOutput = INVALID_HANDLE_VALUE;
-static HANDLE BiosGraphicsOutput = NULL;
-static LPVOID ConsoleFramebuffer = NULL;
-static HANDLE ConsoleMutex = NULL;
-static BYTE CurrentVideoMode, CurrentVideoPage;
-static BOOLEAN VideoNeedsUpdate = TRUE;
-static SMALL_RECT UpdateRectangle = { 0, 0, 0, 0 };
 static CONSOLE_SCREEN_BUFFER_INFO BiosSavedBufferInfo;
 
-static VIDEO_MODE VideoModes[] =
+/*
+ * VGA Register Configurations for BIOS Video Modes
+ * The configurations come from DosBox.
+ */
+static VGA_REGISTERS VideoMode_40x25_text =
 {
-    /* Width | Height | Text | Bpp   | Gray | Pages | Segment */
-    { 40,       25,     TRUE,   16,     TRUE,   8,      0xB800}, /* Mode 00h */
-    { 40,       25,     TRUE,   16,     FALSE,  8,      0xB800}, /* Mode 01h */
-    { 80,       25,     TRUE,   16,     TRUE,   8,      0xB800}, /* Mode 02h */
-    { 80,       25,     TRUE,   16,     FALSE,  8,      0xB800}, /* Mode 03h */
-    { 320,      200,    FALSE,  2,      FALSE,  1,      0xB800}, /* Mode 04h */
-    { 320,      200,    FALSE,  2,      TRUE,   1,      0xB800}, /* Mode 05h */
-    { 640,      200,    FALSE,  1,      FALSE,  1,      0xB800}, /* Mode 06h */
-    { 80,       25,     TRUE,   8,      FALSE,  1,      0xB000}, /* Mode 07h */
-    { 0,        0,      FALSE,  0,      FALSE,  0,      0x0000}, /* Mode 08h - not used */
-    { 0,        0,      FALSE,  0,      FALSE,  0,      0x0000}, /* Mode 09h - not used */
-    { 0,        0,      FALSE,  0,      FALSE,  0,      0x0000}, /* Mode 0Ah - not used */
-    { 0,        0,      FALSE,  0,      FALSE,  0,      0x0000}, /* Mode 0Bh - not used */
-    { 0,        0,      FALSE,  0,      FALSE,  0,      0x0000}, /* Mode 0Ch - not used */
-    { 320,      200,    FALSE,  4,      FALSE,  1,      0xA000}, /* Mode 0Dh */
-    { 640,      200,    FALSE,  4,      FALSE,  1,      0xA000}, /* Mode 0Eh */
-    { 640,      350,    FALSE,  1,      FALSE,  1,      0xA000}, /* Mode 0Fh */
-    { 640,      350,    FALSE,  4,      FALSE,  1,      0xA000}, /* Mode 10h */
-    { 640,      480,    FALSE,  1,      FALSE,  1,      0xA000}, /* Mode 11h */
-    { 640,      480,    FALSE,  4,      FALSE,  1,      0xA000}, /* Mode 12h */
-    { 320,      200,    FALSE,  8,      FALSE,  1,      0xA000}  /* Mode 13h */
+    /* Miscellaneous Register */
+    0x67,
+
+    /* Sequencer Registers */
+    {0x00, 0x08, 0x03, 0x00, 0x07},
+
+    /* CRTC Registers */
+    {0x2D, 0x27, 0x28, 0x90, 0x2B, 0xA0, 0xBF, 0x1F, 0x00, 0x4F, 0x0D, 0x0E,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x14, 0x1F, 0x96, 0xB9, 0xA3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0E, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+     0x3C, 0x3D, 0x3E, 0x3F, 0x0C, 0x00, 0x0F, 0x08, 0x00}
 };
 
-/* PRIVATE FUNCTIONS **********************************************************/
+static VGA_REGISTERS VideoMode_80x25_text =
+{
+    /* Miscellaneous Register */
+    0x67,
+
+    /* Sequencer Registers */
+    {0x00, 0x00, 0x03, 0x00, 0x07},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x55, 0x81, 0xBF, 0x1F, 0x00, 0x4F, 0x0D, 0x0E,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x28, 0x1F, 0x96, 0xB9, 0xA3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0E, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+     0x3C, 0x3D, 0x3E, 0x3F, 0x0C, 0x00, 0x0F, 0x08, 0x00}
+};
 
-static DWORD BiosGetVideoPageSize()
+static VGA_REGISTERS VideoMode_320x200_4color =
 {
-    INT i;
-    DWORD BufferSize = VideoModes[CurrentVideoMode].Width
-                       * VideoModes[CurrentVideoMode].Height
-                       * VideoModes[CurrentVideoMode].Bpp
-                       / 8;
-    
-    for (i = 0; i < 32; i++) if ((1 << i) >= BufferSize) break;
-
-    return 1 << i;
-}
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    {0x00, 0x09, 0x00, 0x00, 0x02},
 
-static BYTE BiosVideoAddressToPage(ULONG Address)
+    /* CRTC Registers */
+    {0x2D, 0x27, 0x28, 0x90, 0x2B, 0x80, 0xBF, 0x1F, 0x00, 0xC1, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x14, 0x00, 0x96, 0xB9, 0xA2,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0F, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x13, 0x15, 0x17, 0x02, 0x04, 0x06, 0x07, 0x10, 0x11, 0x12, 0x13,
+     0x14, 0x15, 0x16, 0x17, 0x01, 0x00, 0x0F, 0x00, 0x00}
+};
+
+static VGA_REGISTERS VideoMode_640x200_2color =
 {
-    return (Address - BiosGetVideoMemoryStart())
-            / BiosGetVideoPageSize();
-}
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    {0x00, 0x09, 0x0F, 0x00, 0x02},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0xBF, 0x1F, 0x00, 0xC1, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x28, 0x00, 0x96, 0xB9, 0xC2,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17,
+     0x17, 0x17, 0x17, 0x17, 0x01, 0x00, 0x01, 0x00, 0x00}
+};
 
-static COORD BiosVideoAddressToCoord(ULONG Address)
+static VGA_REGISTERS VideoMode_320x200_16color =
 {
-    COORD Result = {0, 0};
-    DWORD PageStart = BiosVideoAddressToPage(Address) * BiosGetVideoPageSize();
-    DWORD Offset = Address - BiosGetVideoMemoryStart() - PageStart;
+    /* Miscellaneous Register */
+    0x63,
 
-    if (VideoModes[CurrentVideoMode].Text)
-    {
-        Result.X = (Offset / sizeof(WORD)) % VideoModes[CurrentVideoMode].Width;
-        Result.Y = (Offset / sizeof(WORD)) / VideoModes[CurrentVideoMode].Width;
-    }
-    else
-    {
-        Result.X = ((Offset * 8) / VideoModes[CurrentVideoMode].Bpp)
-                   % VideoModes[CurrentVideoMode].Width;
-        Result.Y = ((Offset * 8) / VideoModes[CurrentVideoMode].Bpp)
-                   / VideoModes[CurrentVideoMode].Width;
-    }
+    /* Sequencer Registers */
+    {0x00, 0x09, 0x0F, 0x00, 0x02},
 
-    return Result;
-}
+    /* CRTC Registers */
+    {0x2D, 0x27, 0x28, 0x90, 0x2B, 0x80, 0xBF, 0x1F, 0x00, 0xC0, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x14, 0x00, 0x96, 0xB9, 0xE3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+     0x3C, 0x3D, 0x3E, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00}
+};
+
+static VGA_REGISTERS VideoMode_640x200_16color =
+{
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    {0x00, 0x01, 0x0F, 0x00, 0x02},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0xBF, 0x1F, 0x00, 0xC0, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x28, 0x00, 0x96, 0xB9, 0xE3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+     0x3C, 0x3D, 0x3E, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00}
+};
+
+static VGA_REGISTERS VideoMode_640x350_16color =
+{
+    /* Miscellaneous Register */
+    0xA3,
+
+    /* Sequencer Registers */
+    {0x00, 0x01, 0x0F, 0x00, 0x02},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0xBF, 0x1F, 0x00, 0x40, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0x83, 0x85, 0x5D, 0x28, 0x0F, 0x63, 0xBA, 0xE3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+     0x3C, 0x3D, 0x3E, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00}
+};
+
+static VGA_REGISTERS VideoMode_640x480_2color =
+{
+    /* Miscellaneous Register */
+    0xE3,
+
+    /* Sequencer Registers */
+    {0x00, 0x01, 0x0F, 0x00, 0x02},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0x0B, 0x3E, 0x00, 0x40, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0xEA, 0x8C, 0xDF, 0x28, 0x00, 0xE7, 0x04, 0xC3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+     0x3F, 0x3F, 0x3F, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00}
+};
+
+static VGA_REGISTERS VideoMode_640x480_16color =
+{
+    /* Miscellaneous Register */
+    0xE3,
+
+    /* Sequencer Registers */
+    {0x00, 0x01, 0x0F, 0x00, 0x02},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0x0B, 0x3E, 0x00, 0x40, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0xEA, 0x8C, 0xDF, 0x28, 0x00, 0xE7, 0x04, 0xE3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+     0x3C, 0x3D, 0x3E, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00}
+};
+
+static VGA_REGISTERS VideoMode_320x200_256color =
+{
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    {0x00, 0x01, 0x0F, 0x00, 0x0E},
+
+    /* CRTC Registers */
+    {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0xBF, 0x1F, 0x00, 0x41, 0x00, 0x00,
+     0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x28, 0x40, 0x96, 0xB9, 0xA3,
+     0xFF},
+
+    /* GC Registers */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F, 0xFF},
+
+    /* AC Registers */
+    {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+     0x0C, 0x0D, 0x0E, 0x0F, 0x41, 0x00, 0x0F, 0x00, 0x00}
+};
+
+/* See http://wiki.osdev.org/Drawing_In_Protected_Mode#Locating_Video_Memory */
+static PVGA_REGISTERS VideoModes[BIOS_MAX_VIDEO_MODE + 1] =
+{
+    &VideoMode_40x25_text,          /* Mode 00h */      // 16 color (mono)
+    &VideoMode_40x25_text,          /* Mode 01h */      // 16 color
+    &VideoMode_80x25_text,          /* Mode 02h */      // 16 color (mono)
+    &VideoMode_80x25_text,          /* Mode 03h */      // 16 color
+    &VideoMode_320x200_4color,      /* Mode 04h */      // 4 color
+    &VideoMode_320x200_4color,      /* Mode 05h */      // same (m)
+    &VideoMode_640x200_2color,      /* Mode 06h */      // 640*200 2 color
+    NULL,                           /* Mode 07h */      // MDA monochrome text 80*25
+    NULL,                           /* Mode 08h */      // PCjr
+    NULL,                           /* Mode 09h */      // PCjr
+    NULL,                           /* Mode 0Ah */      // PCjr
+    NULL,                           /* Mode 0Bh */      // Reserved
+    NULL,                           /* Mode 0Ch */      // Reserved
+    &VideoMode_320x200_16color,     /* Mode 0Dh */      // EGA 320*200 16 color
+    &VideoMode_640x200_16color,     /* Mode 0Eh */      // EGA 640*200 16 color
+    NULL,                           /* Mode 0Fh */      // EGA 640*350 mono
+    &VideoMode_640x350_16color,     /* Mode 10h */      // EGA 640*350 HiRes 16 color
+    &VideoMode_640x480_2color,      /* Mode 11h */      // VGA 640*480 mono
+    &VideoMode_640x480_16color,     /* Mode 12h */      // VGA
+    &VideoMode_320x200_256color,    /* Mode 13h */      // VGA
+};
+
+// FIXME: Are they computable with the previous data ??
+// Values taken from DosBox
+static WORD VideoModePageSize[BIOS_MAX_VIDEO_MODE + 1] =
+{
+    0x0800, 0x0800, 0x1000, 0x1000,
+    0x4000, 0x4000, 0x4000, 0x1000,
+    0x0000, 0x0000, 0x0000, 0x0000,
+    0x0000, 0x2000, 0x4000, 0x8000,
+    0x8000, 0xA000, 0xA000, 0x2000
+};
+
+/* PRIVATE FUNCTIONS **********************************************************/
 
 static BOOLEAN BiosKbdBufferPush(WORD Data)
 {
-    /* Get the location of the element after the head */
-    WORD NextElement = Bda->KeybdBufferHead + 2;
+    /* Get the location of the element after the tail */
+    WORD NextElement = Bda->KeybdBufferTail + sizeof(WORD);
 
     /* Wrap it around if it's at or beyond the end */
     if (NextElement >= Bda->KeybdBufferEnd) NextElement = Bda->KeybdBufferStart;
 
     /* If it's full, fail */
-    if (NextElement == Bda->KeybdBufferTail) return FALSE;
+    if (NextElement == Bda->KeybdBufferHead) return FALSE;
 
     /* Put the value in the queue */
     *((LPWORD)((ULONG_PTR)Bda + Bda->KeybdBufferTail)) = Data;
@@ -135,7 +328,7 @@ static BOOLEAN BiosKbdBufferTop(LPWORD Data)
     return TRUE;
 }
 
-static BOOLEAN BiosKbdBufferPop()
+static BOOLEAN BiosKbdBufferPop(VOID)
 {
     /* If it's empty, fail */
     if (Bda->KeybdBufferHead == Bda->KeybdBufferTail) return FALSE;
@@ -154,303 +347,349 @@ static BOOLEAN BiosKbdBufferPop()
     return TRUE;
 }
 
-static BOOLEAN BiosCreateGraphicsBuffer(BYTE ModeNumber)
+static VOID BiosReadWindow(LPWORD Buffer, SMALL_RECT Rectangle, BYTE Page)
 {
-    INT i;
-    CONSOLE_GRAPHICS_BUFFER_INFO GraphicsBufferInfo;
-    LPBITMAPINFO BitmapInfo;
-    LPWORD PaletteIndex;
-
-    /* Allocate a bitmap info structure */
-    BitmapInfo = (LPBITMAPINFO)HeapAlloc(GetProcessHeap(),
-                                         HEAP_ZERO_MEMORY,
-                                         sizeof(BITMAPINFOHEADER)
-                                         + (1 << VideoModes[ModeNumber].Bpp)
-                                         * sizeof(WORD));
-    if (BitmapInfo == NULL) return FALSE;
-
-    /* Fill the bitmap info header */
-    ZeroMemory(&BitmapInfo->bmiHeader, sizeof(BITMAPINFOHEADER));
-    BitmapInfo->bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
-    BitmapInfo->bmiHeader.biWidth = VideoModes[ModeNumber].Width;
-    BitmapInfo->bmiHeader.biHeight = VideoModes[ModeNumber].Height;
-    BitmapInfo->bmiHeader.biPlanes = 1;
-    BitmapInfo->bmiHeader.biCompression = BI_RGB;
-    BitmapInfo->bmiHeader.biBitCount = VideoModes[ModeNumber].Bpp;
-
-    /* Calculate the image size */
-    BitmapInfo->bmiHeader.biSizeImage = BitmapInfo->bmiHeader.biWidth
-                                        * BitmapInfo->bmiHeader.biHeight
-                                        * (BitmapInfo->bmiHeader.biBitCount >> 3);
-
-    /* Fill the palette data */
-    PaletteIndex = (LPWORD)((ULONG_PTR)BitmapInfo + sizeof(BITMAPINFOHEADER));
-    for (i = 0; i < (1 << VideoModes[ModeNumber].Bpp); i++)
-    {
-        PaletteIndex[i] = i;
-    }
-
-    /* Fill the console graphics buffer info */
-    GraphicsBufferInfo.dwBitMapInfoLength = sizeof(BITMAPINFOHEADER)
-                                            + (1 << VideoModes[ModeNumber].Bpp)
-                                            * sizeof(WORD);
-    GraphicsBufferInfo.lpBitMapInfo = BitmapInfo;
-    GraphicsBufferInfo.dwUsage = DIB_PAL_COLORS;
+    INT i, j;
+    INT Counter = 0;
+    WORD Character;
+    DWORD VideoAddress = TO_LINEAR(TEXT_VIDEO_SEG, Page * Bda->VideoPageSize);
 
-    /* Create the buffer */
-    BiosGraphicsOutput = CreateConsoleScreenBuffer(GENERIC_READ | GENERIC_WRITE,
-                                                   FILE_SHARE_READ | FILE_SHARE_WRITE,
-                                                   NULL,
-                                                   CONSOLE_GRAPHICS_BUFFER,
-                                                   &GraphicsBufferInfo);
-
-    /* Save the framebuffer address and mutex */
-    ConsoleFramebuffer = GraphicsBufferInfo.lpBitMap;
-    ConsoleMutex = GraphicsBufferInfo.hMutex;
-
-    /* Free the bitmap information */
-    HeapFree(GetProcessHeap(), 0, BitmapInfo);
+    for (i = Rectangle.Top; i <= Rectangle.Bottom; i++)
+    {
+        for (j = Rectangle.Left; j <= Rectangle.Right; j++)
+        {
+            /* Read from video memory */
+            VgaReadMemory(VideoAddress + (i * Bda->ScreenColumns + j) * sizeof(WORD),
+                          (LPVOID)&Character,
+                          sizeof(WORD));
 
-    return TRUE;
+            /* Write the data to the buffer in row order */
+            Buffer[Counter++] = Character;
+        }
+    }
 }
 
-static VOID BiosDestroyGraphicsBuffer()
+static VOID BiosWriteWindow(LPWORD Buffer, SMALL_RECT Rectangle, BYTE Page)
 {
-    CloseHandle(ConsoleMutex);
-    CloseHandle(BiosGraphicsOutput);
-}
+    INT i, j;
+    INT Counter = 0;
+    WORD Character;
+    DWORD VideoAddress = TO_LINEAR(TEXT_VIDEO_SEG, Page * Bda->VideoPageSize);
 
-/* PUBLIC FUNCTIONS ***********************************************************/
+    for (i = Rectangle.Top; i <= Rectangle.Bottom; i++)
+    {
+        for (j = Rectangle.Left; j <= Rectangle.Right; j++)
+        {
+            Character = Buffer[Counter++];
 
-BYTE BiosGetVideoMode()
-{
-    return CurrentVideoMode;
+            /* Write to video memory */
+            VgaWriteMemory(VideoAddress + (i * Bda->ScreenColumns + j) * sizeof(WORD),
+                           (LPVOID)&Character,
+                           sizeof(WORD));
+        }
+    }
 }
 
-BOOLEAN BiosSetVideoMode(BYTE ModeNumber)
+static BOOLEAN VgaSetRegisters(PVGA_REGISTERS Registers)
 {
-    COORD Coord;
-
-    /* Make sure this is a valid video mode */
-    if (ModeNumber > BIOS_MAX_VIDEO_MODE) return FALSE;
-    if (VideoModes[ModeNumber].Pages == 0) return FALSE;
+    INT i;
 
-    /* Set the new video mode size */
-    Coord.X = VideoModes[ModeNumber].Width;
-    Coord.Y = VideoModes[ModeNumber].Height;
+    if (Registers == NULL) return FALSE;
 
-    if (VideoModes[ModeNumber].Text && VideoModes[CurrentVideoMode].Text)
-    {
-        /* Switching from text mode to another text mode */
+    /* Disable interrupts */
+    setIF(0);
 
-        /* Resize the text buffer */
-        SetConsoleScreenBufferSize(BiosConsoleOutput, Coord);
-    }
-    else if (VideoModes[ModeNumber].Text && !VideoModes[CurrentVideoMode].Text)
-    {
-        /* Switching from graphics mode to text mode */
+    /*
+     * Set the CRT base address according to the selected mode,
+     * monochrome or color. The following macros:
+     * VGA_INSTAT1_READ, VGA_CRTC_INDEX and VGA_CRTC_DATA are then
+     * used to access the correct VGA I/O ports.
+     */
+    Bda->CrtBasePort = (Registers->Misc & 0x01) ? VGA_CRTC_INDEX_COLOR
+                                                : VGA_CRTC_INDEX_MONO;
 
-        /* Resize the text buffer */
-        SetConsoleScreenBufferSize(BiosConsoleOutput, Coord);
+    /* Write the misc register */
+    VgaWritePort(VGA_MISC_WRITE, Registers->Misc);
 
-        /* Change the active screen buffer to the text buffer */
-        SetConsoleActiveScreenBuffer(BiosConsoleOutput);
+    /* Synchronous reset on */
+    VgaWritePort(VGA_SEQ_INDEX, VGA_SEQ_RESET_REG);
+    VgaWritePort(VGA_SEQ_DATA , VGA_SEQ_RESET_AR);
 
-        /* Cleanup the graphics buffer */
-        BiosDestroyGraphicsBuffer();
+    /* Write the sequencer registers */
+    for (i = 1; i < VGA_SEQ_MAX_REG; i++)
+    {
+        VgaWritePort(VGA_SEQ_INDEX, i);
+        VgaWritePort(VGA_SEQ_DATA, Registers->Sequencer[i]);
     }
-    else if (!VideoModes[ModeNumber].Text && VideoModes[CurrentVideoMode].Text)
+
+    /* Synchronous reset off */
+    VgaWritePort(VGA_SEQ_INDEX, VGA_SEQ_RESET_REG);
+    VgaWritePort(VGA_SEQ_DATA , VGA_SEQ_RESET_SR | VGA_SEQ_RESET_AR);
+
+    /* Unlock CRTC registers 0-7 */
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_END_HORZ_BLANKING_REG);
+    VgaWritePort(VGA_CRTC_DATA, VgaReadPort(VGA_CRTC_DATA) | 0x80);
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_VERT_RETRACE_END_REG);
+    VgaWritePort(VGA_CRTC_DATA, VgaReadPort(VGA_CRTC_DATA) & ~0x80);
+    // Make sure they remain unlocked
+    Registers->CRT[VGA_CRTC_END_HORZ_BLANKING_REG] |= 0x80;
+    Registers->CRT[VGA_CRTC_VERT_RETRACE_END_REG] &= ~0x80;
+
+    /* Write the CRTC registers */
+    for (i = 0; i < VGA_CRTC_MAX_REG; i++)
     {
-        /* Switching from text mode to graphics mode */
-        if (!BiosCreateGraphicsBuffer(ModeNumber)) return FALSE;
+        VgaWritePort(VGA_CRTC_INDEX, i);
+        VgaWritePort(VGA_CRTC_DATA, Registers->CRT[i]);
+    }
 
-        SetConsoleActiveScreenBuffer(BiosGraphicsOutput);
+    /* Write the GC registers */
+    for (i = 0; i < VGA_GC_MAX_REG; i++)
+    {
+        VgaWritePort(VGA_GC_INDEX, i);
+        VgaWritePort(VGA_GC_DATA, Registers->Graphics[i]);
     }
-    else if (!VideoModes[ModeNumber].Text && !VideoModes[CurrentVideoMode].Text)
+
+    /* Write the AC registers */
+    for (i = 0; i < VGA_AC_MAX_REG; i++)
     {
-        /* Switching from graphics mode to another graphics mode */
-    
-        /* Temporarily switch to the text mode buffer */
-        SetConsoleActiveScreenBuffer(BiosConsoleOutput);
+        VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+        VgaWritePort(VGA_AC_INDEX, i);
+        VgaWritePort(VGA_AC_WRITE, Registers->Attribute[i]);
+    }
 
-        /* Cleanup the current graphics mode buffer */
-        BiosDestroyGraphicsBuffer();
+    /* Set the PEL mask */
+    VgaWritePort(VGA_DAC_MASK, 0xFF);
 
-        /* Create a new graphics mode buffer */
-        if (!BiosCreateGraphicsBuffer(ModeNumber)) return FALSE;
+    /* Enable screen and disable palette access */
+    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+    VgaWritePort(VGA_AC_INDEX, 0x20);
 
-        /* Switch to it */
-        SetConsoleActiveScreenBuffer(BiosGraphicsOutput);
-    }
+    /* Enable interrupts */
+    setIF(1);
 
-    /* Change the mode number */
-    CurrentVideoMode = ModeNumber;
-    CurrentVideoPage = 0;
+    return TRUE;
+}
 
-    /* Update the BDA */
-    Bda->VideoMode = CurrentVideoMode;
-    Bda->VideoPage = CurrentVideoPage;
-    Bda->VideoPageSize = BiosGetVideoPageSize();
-    Bda->VideoPageOffset = 0;
-    Bda->ScreenColumns = VideoModes[ModeNumber].Width;
+/* PUBLIC FUNCTIONS ***********************************************************/
 
-    return TRUE;
+BYTE BiosGetVideoMode(VOID)
+{
+    return Bda->VideoMode;
 }
 
-BOOLEAN BiosSetVideoPage(BYTE PageNumber)
+BOOLEAN BiosSetVideoMode(BYTE ModeNumber)
 {
-    ULONG PageStart;
-    COORD Coordinates;
-    CONSOLE_SCREEN_BUFFER_INFO BufferInfo;
+    INT i;
+    BYTE Page;
 
-    /* Make sure this is a valid page number */
-    if (PageNumber >= VideoModes[CurrentVideoMode].Pages) return FALSE;
+    COORD Resolution;
+    PVGA_REGISTERS VgaMode = VideoModes[ModeNumber];
 
-    /* Save the current console buffer in the video memory */
-    PageStart = BiosGetVideoMemoryStart() + CurrentVideoPage * BiosGetVideoPageSize();
-    BiosUpdateVideoMemory(PageStart, PageStart + BiosGetVideoPageSize());
+    DPRINT1("Switching to mode %Xh; VgaMode = 0x%p\n", ModeNumber, VgaMode);
 
-    /* Save the cursor */
-    if (!GetConsoleScreenBufferInfo(BiosConsoleOutput, &BufferInfo)) return FALSE;
-    Bda->CursorPosition[CurrentVideoPage] = MAKEWORD(BufferInfo.dwCursorPosition.X,
-                                                     BufferInfo.dwCursorPosition.Y);
+    if (!VgaSetRegisters(VgaMode)) return FALSE;
 
-    /* Set the page */
-    CurrentVideoPage = PageNumber;
+    // /* Disable screen and enable palette access */
+    // VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+    // VgaWritePort(VGA_AC_INDEX, 0x00);
 
-    /* Update the BDA */
-    Bda->VideoPage = CurrentVideoPage;
-    Bda->VideoPageSize = BiosGetVideoPageSize();
-    Bda->VideoPageOffset = CurrentVideoPage * Bda->VideoPageSize;
+    if ((ModeNumber == 0x0D) || (ModeNumber == 0x0E) || (ModeNumber == 0x10))
+    {
+        /* EGA modes */
+        extern CONST COLORREF EgaPalette[VGA_MAX_COLORS / 4];
+        for (i = 0; i < sizeof(EgaPalette)/sizeof(EgaPalette[0]); i++)
+        {
+            VgaWritePort(VGA_DAC_WRITE_INDEX, i);
+            VgaWritePort(VGA_DAC_DATA, VGA_COLOR_TO_DAC(GetRValue(EgaPalette[i])));
+            VgaWritePort(VGA_DAC_DATA, VGA_COLOR_TO_DAC(GetGValue(EgaPalette[i])));
+            VgaWritePort(VGA_DAC_DATA, VGA_COLOR_TO_DAC(GetBValue(EgaPalette[i])));
+        }
+    }
+    else
+    {
+        /* Reset the palette */
+        VgaResetPalette();
+    }
 
-    /* Update the console */
-    PageStart = BiosGetVideoMemoryStart() + Bda->VideoPage * BiosGetVideoPageSize();
-    BiosUpdateConsole(PageStart, PageStart + BiosGetVideoPageSize());
+    // /* Enable screen and disable palette access */
+    // VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+    // VgaWritePort(VGA_AC_INDEX, 0x20);
 
-    /* Set the cursor */
-    Coordinates.X = LOBYTE(Bda->CursorPosition[Bda->VideoPage]);
-    Coordinates.Y = HIBYTE(Bda->CursorPosition[Bda->VideoPage]);
-    SetConsoleCursorPosition(BiosConsoleOutput, Coordinates);
+    // Bda->CrtModeControl;
+    // Bda->CrtColorPaletteMask;
+    // Bda->EGAFlags;
+    // Bda->VGAFlags;
 
-    return TRUE;
-}
+    /* Update the values in the BDA */
+    Bda->VideoMode       = ModeNumber;
+    Bda->VideoPageSize   = VideoModePageSize[ModeNumber];
+    Bda->VideoPage       = 0;
+    Bda->VideoPageOffset = Bda->VideoPage * Bda->VideoPageSize;
 
-inline DWORD BiosGetVideoMemoryStart()
-{
-    return (VideoModes[CurrentVideoMode].Segment << 4);
-}
+    /* Set the start address in the CRTC */
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_START_ADDR_LOW_REG);
+    VgaWritePort(VGA_CRTC_DATA , LOBYTE(Bda->VideoPageOffset));
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_START_ADDR_HIGH_REG);
+    VgaWritePort(VGA_CRTC_DATA , HIBYTE(Bda->VideoPageOffset));
 
-inline VOID BiosVerticalRefresh()
-{
-    /* Ignore if we're in text mode */
-    if (VideoModes[CurrentVideoMode].Text) return;
+    /* Get the character height */
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_MAX_SCAN_LINE_REG);
+    Bda->CharacterHeight = 1 + (VgaReadPort(VGA_CRTC_DATA) & 0x1F);
 
-    /* Ignore if there's nothing to update */
-    if (!VideoNeedsUpdate) return;
+    Resolution = VgaGetDisplayResolution();
+    Bda->ScreenColumns = Resolution.X;
+    Bda->ScreenRows    = Resolution.Y - 1;
 
-    /* Redraw the screen */
-    InvalidateConsoleDIBits(BiosGraphicsOutput, &UpdateRectangle);
+    /* Set cursor position for each page */
+    for (Page = 0; Page < BIOS_MAX_PAGES; ++Page)
+        BiosSetCursorPosition(0, 0, Page);
 
-    /* Clear the update flag */
-    VideoNeedsUpdate = FALSE;
+    return TRUE;
 }
 
-BOOLEAN BiosInitialize()
+BOOLEAN BiosSetVideoPage(BYTE PageNumber)
 {
-    INT i;
-    WORD Offset = 0;
-    LPWORD IntVecTable = (LPWORD)((ULONG_PTR)BaseAddress);
-    LPBYTE BiosCode = (LPBYTE)((ULONG_PTR)BaseAddress + TO_LINEAR(BIOS_SEGMENT, 0));
-
-    /* Initialize the BDA */
-    Bda = (PBIOS_DATA_AREA)((ULONG_PTR)BaseAddress + TO_LINEAR(BDA_SEGMENT, 0));
-    Bda->EquipmentList = BIOS_EQUIPMENT_LIST;
-    Bda->KeybdBufferStart = FIELD_OFFSET(BIOS_DATA_AREA, KeybdBuffer);
-    Bda->KeybdBufferEnd = Bda->KeybdBufferStart + BIOS_KBD_BUFFER_SIZE * sizeof(WORD);
+    BYTE Row, Column;
 
-    /* Generate ISR stubs and fill the IVT */
-    for (i = 0; i < 256; i++)
-    {
-        IntVecTable[i * 2] = Offset;
-        IntVecTable[i * 2 + 1] = BIOS_SEGMENT;
+    /* Check if the page exists */
+    if (PageNumber >= BIOS_MAX_PAGES) return FALSE;
 
-        if (i != SPECIAL_INT_NUM)
-        {
-            BiosCode[Offset++] = 0xFA; // cli
+    /* Check if this is the same page */
+    if (PageNumber == Bda->VideoPage) return TRUE;
 
-            BiosCode[Offset++] = 0x6A; // push i
-            BiosCode[Offset++] = (BYTE)i;
+    /* Update the values in the BDA */
+    Bda->VideoPage       = PageNumber;
+    Bda->VideoPageOffset = Bda->VideoPage * Bda->VideoPageSize;
 
-            BiosCode[Offset++] = 0xCD; // int SPECIAL_INT_NUM
-            BiosCode[Offset++] = SPECIAL_INT_NUM;
+    /* Set the start address in the CRTC */
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_START_ADDR_LOW_REG);
+    VgaWritePort(VGA_CRTC_DATA , LOBYTE(Bda->VideoPageOffset));
+    VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_START_ADDR_HIGH_REG);
+    VgaWritePort(VGA_CRTC_DATA , HIBYTE(Bda->VideoPageOffset));
 
-            BiosCode[Offset++] = 0x83; // add sp, 2
-            BiosCode[Offset++] = 0xC4;
-            BiosCode[Offset++] = 0x02;
-        }
+    /*
+     * Get the cursor location (we don't update anything on the BIOS side
+     * but we update the cursor location on the VGA side).
+     */
+    BiosGetCursorPosition(&Row, &Column, PageNumber);
+    BiosSetCursorPosition(Row, Column, PageNumber);
 
-        BiosCode[Offset++] = 0xCF; // iret
-    }
-
-    /* Get the input and output handles to the real console */
-    BiosConsoleInput = CreateFile(TEXT("CONIN$"),
-                                  GENERIC_READ | GENERIC_WRITE,
-                                  FILE_SHARE_READ | FILE_SHARE_WRITE,
-                                  NULL,
-                                  OPEN_EXISTING,
-                                  0,
-                                  NULL);
+    return TRUE;
+}
 
-    BiosConsoleOutput = CreateFile(TEXT("CONOUT$"),
+BOOLEAN BiosInitialize(VOID)
+{
+    /* Initialize the BDA */
+    Bda = (PBIOS_DATA_AREA)SEG_OFF_TO_PTR(BDA_SEGMENT, 0);
+    Bda->EquipmentList = BIOS_EQUIPMENT_LIST;
+    /*
+     * Conventional memory size is 640 kB,
+     * see: http://webpages.charter.net/danrollins/techhelp/0184.HTM
+     * and see Ralf Brown: http://www.ctyme.com/intr/rb-0598.htm
+     * for more information.
+     */
+    Bda->MemorySize = 0x0280;
+    Bda->KeybdBufferStart = FIELD_OFFSET(BIOS_DATA_AREA, KeybdBuffer);
+    Bda->KeybdBufferEnd = Bda->KeybdBufferStart + BIOS_KBD_BUFFER_SIZE * sizeof(WORD);
+    Bda->KeybdBufferHead = Bda->KeybdBufferTail = 0;
+
+    /* Initialize the 32-bit Interrupt system */
+    InitializeInt32(BIOS_SEGMENT);
+
+    /* Register the BIOS 32-bit Interrupts */
+    RegisterInt32(BIOS_VIDEO_INTERRUPT    , BiosVideoService        );
+    RegisterInt32(BIOS_EQUIPMENT_INTERRUPT, BiosEquipmentService    );
+    RegisterInt32(BIOS_MEMORY_SIZE        , BiosGetMemorySize       );
+    RegisterInt32(BIOS_MISC_INTERRUPT     , BiosMiscService         );
+    RegisterInt32(BIOS_KBD_INTERRUPT      , BiosKeyboardService     );
+    RegisterInt32(BIOS_TIME_INTERRUPT     , BiosTimeService         );
+    RegisterInt32(BIOS_SYS_TIMER_INTERRUPT, BiosSystemTimerInterrupt);
+
+    /* Some interrupts are in fact addresses to tables */
+    ((PDWORD)BaseAddress)[0x1D] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x1E] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x1F] = (DWORD)NULL;
+
+    ((PDWORD)BaseAddress)[0x41] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x43] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x44] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x46] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x48] = (DWORD)NULL;
+    ((PDWORD)BaseAddress)[0x49] = (DWORD)NULL;
+
+    /* Get the input handle to the real console, and check for success */
+    BiosConsoleInput = CreateFileW(L"CONIN$",
                                    GENERIC_READ | GENERIC_WRITE,
                                    FILE_SHARE_READ | FILE_SHARE_WRITE,
                                    NULL,
                                    OPEN_EXISTING,
                                    0,
                                    NULL);
+    if (BiosConsoleInput == INVALID_HANDLE_VALUE)
+    {
+        return FALSE;
+    }
 
-    /* Make sure it was successful */
-    if ((BiosConsoleInput == INVALID_HANDLE_VALUE)
-        || (BiosConsoleOutput == INVALID_HANDLE_VALUE))
+    /* Get the output handle to the real console, and check for success */
+    BiosConsoleOutput = CreateFileW(L"CONOUT$",
+                                    GENERIC_READ | GENERIC_WRITE,
+                                    FILE_SHARE_READ | FILE_SHARE_WRITE,
+                                    NULL,
+                                    OPEN_EXISTING,
+                                    0,
+                                    NULL);
+    if (BiosConsoleOutput == INVALID_HANDLE_VALUE)
     {
+        CloseHandle(BiosConsoleInput);
         return FALSE;
     }
 
     /* Save the console screen buffer information */
     if (!GetConsoleScreenBufferInfo(BiosConsoleOutput, &BiosSavedBufferInfo))
     {
+        CloseHandle(BiosConsoleOutput);
+        CloseHandle(BiosConsoleInput);
         return FALSE;
     }
 
-    /* Store the cursor position */
-    Bda->CursorPosition[0] = MAKEWORD(BiosSavedBufferInfo.dwCursorPosition.X,
-                                      BiosSavedBufferInfo.dwCursorPosition.Y);
-    
-    /* Set the default video mode */
-    BiosSetVideoMode(BIOS_DEFAULT_VIDEO_MODE);
+    /* Initialize VGA */
+    if (!VgaInitialize(BiosConsoleOutput))
+    {
+        CloseHandle(BiosConsoleOutput);
+        CloseHandle(BiosConsoleInput);
+        return FALSE;
+    }
+
+    /* Update the cursor position */
+    BiosSetCursorPosition(BiosSavedBufferInfo.dwCursorPosition.Y,
+                          BiosSavedBufferInfo.dwCursorPosition.X,
+                          0);
 
     /* Set the console input mode */
     SetConsoleMode(BiosConsoleInput, ENABLE_MOUSE_INPUT | ENABLE_PROCESSED_INPUT);
 
+    /* Initialize PS2 */
+    PS2Initialize(BiosConsoleInput);
+
     /* Initialize the PIC */
     PicWriteCommand(PIC_MASTER_CMD, PIC_ICW1 | PIC_ICW1_ICW4);
-    PicWriteCommand(PIC_SLAVE_CMD, PIC_ICW1 | PIC_ICW1_ICW4);
+    PicWriteCommand(PIC_SLAVE_CMD , PIC_ICW1 | PIC_ICW1_ICW4);
 
     /* Set the interrupt offsets */
     PicWriteData(PIC_MASTER_DATA, BIOS_PIC_MASTER_INT);
-    PicWriteData(PIC_SLAVE_DATA, BIOS_PIC_SLAVE_INT);
+    PicWriteData(PIC_SLAVE_DATA , BIOS_PIC_SLAVE_INT);
 
     /* Tell the master PIC there is a slave at IRQ 2 */
     PicWriteData(PIC_MASTER_DATA, 1 << 2);
-    PicWriteData(PIC_SLAVE_DATA, 2);
+    PicWriteData(PIC_SLAVE_DATA , 2);
 
     /* Make sure the PIC is in 8086 mode */
     PicWriteData(PIC_MASTER_DATA, PIC_ICW4_8086);
-    PicWriteData(PIC_SLAVE_DATA, PIC_ICW4_8086);
+    PicWriteData(PIC_SLAVE_DATA , PIC_ICW4_8086);
 
     /* Clear the masks for both PICs */
     PicWriteData(PIC_MASTER_DATA, 0x00);
-    PicWriteData(PIC_SLAVE_DATA, 0x00);
+    PicWriteData(PIC_SLAVE_DATA , 0x00);
 
     PitWriteCommand(0x34);
     PitWriteData(0, 0x00);
@@ -459,247 +698,293 @@ BOOLEAN BiosInitialize()
     return TRUE;
 }
 
-VOID BiosCleanup()
+VOID BiosCleanup(VOID)
 {
+    PS2Cleanup();
+
     /* Restore the old screen buffer */
     SetConsoleActiveScreenBuffer(BiosConsoleOutput);
 
     /* Restore the screen buffer size */
     SetConsoleScreenBufferSize(BiosConsoleOutput, BiosSavedBufferInfo.dwSize);
 
-    /* Free the graphics buffer */
-    if (!VideoModes[CurrentVideoMode].Text) BiosDestroyGraphicsBuffer();
-
     /* Close the console handles */
-    if (BiosConsoleInput != INVALID_HANDLE_VALUE) CloseHandle(BiosConsoleInput);
     if (BiosConsoleOutput != INVALID_HANDLE_VALUE) CloseHandle(BiosConsoleOutput);
+    if (BiosConsoleInput  != INVALID_HANDLE_VALUE) CloseHandle(BiosConsoleInput);
 }
 
-VOID BiosUpdateConsole(ULONG StartAddress, ULONG EndAddress)
+WORD BiosPeekCharacter(VOID)
 {
-    ULONG i;
-    COORD Coordinates;
-    COORD Origin = { 0, 0 };
-    COORD UnitSize = { 1, 1 };
-    CHAR_INFO Character;
-    SMALL_RECT Rect;
+    WORD CharacterData = 0;
+
+    /* Get the key from the queue, but don't remove it */
+    if (BiosKbdBufferTop(&CharacterData)) return CharacterData;
+    else return 0xFFFF;
+}
 
-    /* Start from the character address */
-    StartAddress &= ~1;
+WORD BiosGetCharacter(VOID)
+{
+    WORD CharacterData = 0;
 
-    if (VideoModes[CurrentVideoMode].Text)
+    /* Check if there is a key available */
+    if (BiosKbdBufferTop(&CharacterData))
     {
-        /* Loop through all the addresses */
-        for (i = StartAddress; i < EndAddress; i += 2)
-        {
-            /* Get the coordinates */
-            Coordinates = BiosVideoAddressToCoord(i);
-
-            /* Make sure this is the current page */
-            if (BiosVideoAddressToPage(i) != CurrentVideoPage) continue;
-
-            /* Fill the rectangle structure */
-            Rect.Left = Coordinates.X;
-            Rect.Top = Coordinates.Y;
-            Rect.Right = Rect.Left;
-            Rect.Bottom = Rect.Top;
-
-            /* Fill the character data */
-            Character.Char.AsciiChar = *((PCHAR)((ULONG_PTR)BaseAddress + i));
-            Character.Attributes = *((PBYTE)((ULONG_PTR)BaseAddress + i + 1));
-
-            /* Write the character */
-            WriteConsoleOutputA(BiosConsoleOutput,
-                                &Character,
-                                UnitSize,
-                                Origin,
-                                &Rect);
-        }
+        /* A key was available, remove it from the queue */
+        BiosKbdBufferPop();
     }
     else
     {
-        /* Wait for the mutex object */
-        WaitForSingleObject(ConsoleMutex, INFINITE);
+        /* No key available. Set the handler CF to repeat the BOP */
+        setCF(1);
+        // CharacterData = 0xFFFF;
+    }
 
-        /* Copy the data to the framebuffer */
-        RtlCopyMemory((LPVOID)((ULONG_PTR)ConsoleFramebuffer
-                      + StartAddress - BiosGetVideoMemoryStart()),
-                      (LPVOID)((ULONG_PTR)BaseAddress + StartAddress),
-                      EndAddress - StartAddress);
+    return CharacterData;
+}
 
-        /* Release the mutex */
-        ReleaseMutex(ConsoleMutex);
+VOID BiosGetCursorPosition(PBYTE Row, PBYTE Column, BYTE Page)
+{
+    /* Make sure the selected video page is valid */
+    if (Page >= BIOS_MAX_PAGES) return;
 
-        /* Check if this is the first time the rectangle is updated */
-        if (!VideoNeedsUpdate)
-        {
-            UpdateRectangle.Left = UpdateRectangle.Top = (SHORT)0x7FFF;
-            UpdateRectangle.Right = UpdateRectangle.Bottom = (SHORT)0x8000;
-        }
+    /* Get the cursor location */
+    *Row    = HIBYTE(Bda->CursorPosition[Page]);
+    *Column = LOBYTE(Bda->CursorPosition[Page]);
+}
 
-        /* Expand the update rectangle */
-        for (i = StartAddress; i < EndAddress; i++)
-        {
-            /* Get the coordinates */
-            Coordinates = BiosVideoAddressToCoord(i);
-
-            /* Expand the rectangle to include the point */
-            UpdateRectangle.Left = min(UpdateRectangle.Left, Coordinates.X);
-            UpdateRectangle.Right = max(UpdateRectangle.Right, Coordinates.X);
-            UpdateRectangle.Top = min(UpdateRectangle.Top, Coordinates.Y);
-            UpdateRectangle.Bottom = max(UpdateRectangle.Bottom, Coordinates.Y);
-        }
+VOID BiosSetCursorPosition(BYTE Row, BYTE Column, BYTE Page)
+{
+    /* Make sure the selected video page is valid */
+    if (Page >= BIOS_MAX_PAGES) return;
+
+    /* Update the position in the BDA */
+    Bda->CursorPosition[Page] = MAKEWORD(Column, Row);
+
+    /* Check if this is the current video page */
+    if (Page == Bda->VideoPage)
+    {
+        WORD Offset = Row * Bda->ScreenColumns + Column;
 
-        /* Set the update flag */
-        VideoNeedsUpdate = TRUE;
+        /* Modify the CRTC registers */
+        VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_CURSOR_LOC_LOW_REG);
+        VgaWritePort(VGA_CRTC_DATA , LOBYTE(Offset));
+        VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_CURSOR_LOC_HIGH_REG);
+        VgaWritePort(VGA_CRTC_DATA , HIBYTE(Offset));
     }
 }
 
-VOID BiosUpdateVideoMemory(ULONG StartAddress, ULONG EndAddress)
+BOOLEAN BiosScrollWindow(INT Direction,
+                         DWORD Amount,
+                         SMALL_RECT Rectangle,
+                         BYTE Page,
+                         BYTE FillAttribute)
 {
-    ULONG i;
-    COORD Coordinates;
-    WORD Attribute;
-    DWORD CharsWritten;
-
-    if (VideoModes[CurrentVideoMode].Text)
+    DWORD i;
+    LPWORD WindowData;
+    WORD WindowWidth = Rectangle.Right - Rectangle.Left + 1;
+    WORD WindowHeight = Rectangle.Bottom - Rectangle.Top + 1;
+    DWORD WindowSize = WindowWidth * WindowHeight;
+
+    /* Allocate a buffer for the window */
+    WindowData = (LPWORD)HeapAlloc(GetProcessHeap(),
+                                   HEAP_ZERO_MEMORY,
+                                   WindowSize * sizeof(WORD));
+    if (WindowData == NULL) return FALSE;
+
+    /* Read the window data */
+    BiosReadWindow(WindowData, Rectangle, Page);
+
+    if ((Amount == 0)
+        || (((Direction == SCROLL_DIRECTION_UP)
+        || (Direction == SCROLL_DIRECTION_DOWN))
+        && (Amount >= WindowHeight))
+        || (((Direction == SCROLL_DIRECTION_LEFT)
+        || (Direction == SCROLL_DIRECTION_RIGHT))
+        && (Amount >= WindowWidth)))
     {
-        /* Loop through all the addresses */
-        for (i = StartAddress; i < EndAddress; i++)
+        /* Fill the window */
+        for (i = 0; i < WindowSize; i++)
         {
-            /* Get the coordinates */
-            Coordinates = BiosVideoAddressToCoord(i);
+            WindowData[i] = MAKEWORD(' ', FillAttribute);
+        }
 
-            /* Make sure this is the current page */
-            if (BiosVideoAddressToPage(i) != CurrentVideoPage) continue;
+        goto Done;
+    }
+
+    switch (Direction)
+    {
+        case SCROLL_DIRECTION_UP:
+        {
+            RtlMoveMemory(WindowData,
+                          &WindowData[WindowWidth * Amount],
+                          (WindowSize - WindowWidth * Amount) * sizeof(WORD));
 
-            /* Check if this is a character byte or an attribute byte */
-            if ((i - BiosGetVideoMemoryStart()) % 2 == 0)
+            for (i = 0; i < Amount * WindowWidth; i++)
             {
-                /* This is a regular character */
-                ReadConsoleOutputCharacterA(BiosConsoleOutput,
-                                            (LPSTR)((ULONG_PTR)BaseAddress + i),
-                                            sizeof(CHAR),
-                                            Coordinates,
-                                            &CharsWritten);
+                WindowData[WindowSize - i - 1] = MAKEWORD(' ', FillAttribute);
             }
-            else
+
+            break;
+        }
+
+        case SCROLL_DIRECTION_DOWN:
+        {
+            RtlMoveMemory(&WindowData[WindowWidth * Amount],
+                          WindowData,
+                          (WindowSize - WindowWidth * Amount) * sizeof(WORD));
+
+            for (i = 0; i < Amount * WindowWidth; i++)
             {
-                /*  This is an attribute */
-                ReadConsoleOutputAttribute(BiosConsoleOutput,
-                                           &Attribute,
-                                           sizeof(CHAR),
-                                           Coordinates,
-                                           &CharsWritten);
-
-                *(PCHAR)((ULONG_PTR)BaseAddress + i) = LOBYTE(Attribute);
+                WindowData[i] = MAKEWORD(' ', FillAttribute);
             }
+
+            break;
+        }
+
+        default:
+        {
+            // TODO: NOT IMPLEMENTED!
+            UNIMPLEMENTED;
         }
     }
-    else
-    {
-        /* Wait for the mutex object */
-        WaitForSingleObject(ConsoleMutex, INFINITE);
 
-        /* Copy the data to the emulator memory */
-        RtlCopyMemory((LPVOID)((ULONG_PTR)BaseAddress + StartAddress),
-                      (LPVOID)((ULONG_PTR)ConsoleFramebuffer
-                      + StartAddress - BiosGetVideoMemoryStart()),
-                      EndAddress - StartAddress);
+Done:
+    /* Write back the window data */
+    BiosWriteWindow(WindowData, Rectangle, Page);
 
-        /* Release the mutex */
-        ReleaseMutex(ConsoleMutex);
-    }
+    /* Free the window buffer */
+    HeapFree(GetProcessHeap(), 0, WindowData);
+
+    return TRUE;
 }
 
-WORD BiosPeekCharacter()
+VOID BiosPrintCharacter(CHAR Character, BYTE Attribute, BYTE Page)
 {
-    WORD CharacterData;
-    
-    /* Check if there is a key available */
-    if (Bda->KeybdBufferHead == Bda->KeybdBufferTail) return 0xFFFF;
+    WORD CharData = MAKEWORD(Character, Attribute);
+    BYTE Row, Column;
 
-    /* Get the key from the queue, but don't remove it */
-    BiosKbdBufferTop(&CharacterData);
+    /* Make sure the page exists */
+    if (Page >= BIOS_MAX_PAGES) return;
 
-    return CharacterData;
-}
+    /* Get the cursor location */
+    BiosGetCursorPosition(&Row, &Column, Page);
 
-WORD BiosGetCharacter()
-{
-    WORD CharacterData;
-    INPUT_RECORD InputRecord;
-    DWORD Count;
+    if (Character == '\a')
+    {
+        /* Bell control character */
+        // NOTE: We may use what the terminal emulator offers to us...
+        Beep(800, 200);
+        return;
+    }
+    else if (Character == '\b')
+    {
+        /* Backspace control character */
+        if (Column > 0)
+        {
+            Column--;
+        }
+        else if (Row > 0)
+        {
+            Column = Bda->ScreenColumns - 1;
+            Row--;
+        }
 
-    /* Check if there is a key available */
-    if (Bda->KeybdBufferHead != Bda->KeybdBufferTail)
+        /* Erase the existing character */
+        CharData = MAKEWORD(' ', Attribute);
+        EmulatorWriteMemory(&EmulatorContext,
+                            TO_LINEAR(TEXT_VIDEO_SEG,
+                                Page * Bda->VideoPageSize +
+                                (Row * Bda->ScreenColumns + Column) * sizeof(WORD)),
+                            (LPVOID)&CharData,
+                            sizeof(WORD));
+    }
+    else if (Character == '\t')
     {
-        /* Get the key from the queue, and remove it */
-        BiosKbdBufferTop(&CharacterData);
-        BiosKbdBufferPop();
+        /* Horizontal Tabulation control character */
+        do
+        {
+            // Taken from DosBox
+            BiosPrintCharacter(' ', Attribute, Page);
+            BiosGetCursorPosition(&Row, &Column, Page);
+        } while (Column % 8);
+    }
+    else if (Character == '\n')
+    {
+        /* Line Feed control character */
+        Row++;
+    }
+    else if (Character == '\r')
+    {
+        /* Carriage Return control character */
+        Column = 0;
     }
     else
     {
-        while (TRUE)
-        {
-            /* Wait for a console event */
-            WaitForSingleObject(BiosConsoleInput, INFINITE);
-    
-            /* Read the event, and make sure it's a keypress */
-            if (!ReadConsoleInput(BiosConsoleInput, &InputRecord, 1, &Count)) continue;
-            if (InputRecord.EventType != KEY_EVENT) continue;
-            if (!InputRecord.Event.KeyEvent.bKeyDown) continue;
-
-            /* Save the scan code and end the loop */
-            CharacterData = (InputRecord.Event.KeyEvent.wVirtualScanCode << 8)
-                            | InputRecord.Event.KeyEvent.uChar.AsciiChar;
+        /* Default character */
+
+        /* Write the character */
+        EmulatorWriteMemory(&EmulatorContext,
+                            TO_LINEAR(TEXT_VIDEO_SEG,
+                                Page * Bda->VideoPageSize +
+                                (Row * Bda->ScreenColumns + Column) * sizeof(WORD)),
+                            (LPVOID)&CharData,
+                            sizeof(WORD));
+
+        /* Advance the cursor */
+        Column++;
+    }
 
-            break;
-        }
+    /* Check if it passed the end of the row */
+    if (Column >= Bda->ScreenColumns)
+    {
+        /* Return to the first column and go to the next line */
+        Column = 0;
+        Row++;
     }
 
-    return CharacterData;
+    /* Scroll the screen up if needed */
+    if (Row > Bda->ScreenRows)
+    {
+        /* The screen must be scrolled up */
+        SMALL_RECT Rectangle = { 0, 0, Bda->ScreenColumns - 1, Bda->ScreenRows };
+
+        BiosScrollWindow(SCROLL_DIRECTION_UP,
+                         1,
+                         Rectangle,
+                         Page,
+                         DEFAULT_ATTRIBUTE);
+
+        Row--;
+    }
+
+    /* Set the cursor position */
+    BiosSetCursorPosition(Row, Column, Page);
 }
 
-VOID BiosVideoService()
+VOID WINAPI BiosVideoService(LPWORD Stack)
 {
-    INT i, CursorHeight;
-    BOOLEAN Invisible = FALSE;
-    COORD Position;
-    CONSOLE_CURSOR_INFO CursorInfo;
-    CHAR_INFO Character;
-    SMALL_RECT Rect;
-    DWORD Eax = EmulatorGetRegister(EMULATOR_REG_AX);
-    DWORD Ecx = EmulatorGetRegister(EMULATOR_REG_CX);
-    DWORD Edx = EmulatorGetRegister(EMULATOR_REG_DX);
-    DWORD Ebx = EmulatorGetRegister(EMULATOR_REG_BX);
-
-    switch (HIBYTE(Eax))
+    switch (getAH())
     {
         /* Set Video Mode */
         case 0x00:
         {
-            BiosSetVideoMode(LOBYTE(Eax));
+            BiosSetVideoMode(getAL());
+            VgaClearMemory();
             break;
         }
 
         /* Set Text-Mode Cursor Shape */
         case 0x01:
         {
-            /* Retrieve and validate the input */
-            Invisible = ((HIBYTE(Ecx) >> 5) & 0x03) ? TRUE : FALSE;
-            CursorHeight = (HIBYTE(Ecx) & 0x1F) - (LOBYTE(Ecx) & 0x1F);
-            if (CursorHeight < 1) CursorHeight = 1;
-            if (CursorHeight > 100) CursorHeight = 100;
-
             /* Update the BDA */
-            Bda->CursorStartLine = HIBYTE(Ecx);
-            Bda->CursorEndLine = LOBYTE(Ecx) & 0x1F;
+            Bda->CursorStartLine = getCH();
+            Bda->CursorEndLine   = getCL();
 
-            /* Set the cursor */
-            CursorInfo.dwSize = (CursorHeight * 100) / CONSOLE_FONT_HEIGHT;
-            CursorInfo.bVisible = !Invisible;
-            SetConsoleCursorInfo(BiosConsoleOutput, &CursorInfo);
+            /* Modify the CRTC registers */
+            VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_CURSOR_START_REG);
+            VgaWritePort(VGA_CRTC_DATA , Bda->CursorStartLine);
+            VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_CURSOR_END_REG);
+            VgaWritePort(VGA_CRTC_DATA , Bda->CursorEndLine);
 
             break;
         }
@@ -707,20 +992,7 @@ VOID BiosVideoService()
         /* Set Cursor Position */
         case 0x02:
         {
-            /* Make sure the selected video page exists */
-            if (HIBYTE(Ebx) >= VideoModes[CurrentVideoMode].Pages) break;
-
-            Bda->CursorPosition[HIBYTE(Ebx)] = LOWORD(Edx);
-
-            /* Check if this is the current video page */
-            if (HIBYTE(Ebx) == CurrentVideoPage)
-            {
-                /* Yes, change the actual cursor */
-                Position.X = LOBYTE(Edx);
-                Position.Y = HIBYTE(Edx);
-                SetConsoleCursorPosition(BiosConsoleOutput, Position);
-            }
-
+            BiosSetCursorPosition(getDH(), getDL(), getBH());
             break;
         }
 
@@ -728,265 +1000,511 @@ VOID BiosVideoService()
         case 0x03:
         {
             /* Make sure the selected video page exists */
-            if (HIBYTE(Ebx) >= VideoModes[CurrentVideoMode].Pages) break;
+            if (getBH() >= BIOS_MAX_PAGES) break;
 
             /* Return the result */
-            EmulatorSetRegister(EMULATOR_REG_AX, 0);
-            EmulatorSetRegister(EMULATOR_REG_CX,
-                                (Bda->CursorStartLine << 8) | Bda->CursorEndLine);
-            EmulatorSetRegister(EMULATOR_REG_DX, Bda->CursorPosition[HIBYTE(Ebx)]);
+            setAX(0);
+            setCX(MAKEWORD(Bda->CursorEndLine, Bda->CursorStartLine));
+            setDX(Bda->CursorPosition[getBH()]);
+            break;
+        }
 
+        /* Query Light Pen */
+        case 0x04:
+        {
+            /*
+             * On modern BIOSes, this function returns 0
+             * so that we can ignore the other registers.
+             */
+            setAX(0);
             break;
         }
 
         /* Select Active Display Page */
         case 0x05:
         {
-            /* Check if the page exists */
-            if (LOBYTE(Eax) >= VideoModes[CurrentVideoMode].Pages) break;
+            BiosSetVideoPage(getAL());
+            break;
+        }
 
-            /* Check if this is the same page */
-            if (LOBYTE(Eax) == CurrentVideoPage) break;
+        /* Scroll Window Up/Down */
+        case 0x06:
+        case 0x07:
+        {
+            SMALL_RECT Rectangle = { getCL(), getCH(), getDL(), getDH() };
 
-            /* Change the video page */
-            BiosSetVideoPage(LOBYTE(Eax));
+            /* Call the internal function */
+            BiosScrollWindow((getAH() == 0x06) ? SCROLL_DIRECTION_UP
+                                               : SCROLL_DIRECTION_DOWN,
+                             getAL(),
+                             Rectangle,
+                             Bda->VideoPage,
+                             getBH());
 
             break;
         }
 
-        /* Scroll Up/Down Window */
-        // TODO: Implement for different pages
-        case 0x06:
-        case 0x07:
+        /* Read/Write Character From Cursor Position */
+        case 0x08:
+        case 0x09:
+        case 0x0A:
         {
-            BYTE Lines = LOBYTE(Eax);
+            WORD CharacterData = MAKEWORD(getAL(), getBL());
+            BYTE Page = getBH();
+            DWORD Offset;
 
-            Rect.Top = HIBYTE(Ecx);
-            Rect.Left = LOBYTE(Ecx);
-            Rect.Bottom = HIBYTE(Edx);
-            Rect.Right = LOBYTE(Edx);
-            Character.Char.UnicodeChar = L' ';
-            Character.Attributes = HIBYTE(Ebx);
-            Position.X = Rect.Left;
+            /* Check if the page exists */
+            if (Page >= BIOS_MAX_PAGES) break;
 
-            /* 0 means clear entire window */
-            if (Lines == 0) Lines = Rect.Bottom - Rect.Top;
+            /* Find the offset of the character */
+            Offset = Page * Bda->VideoPageSize +
+                     (HIBYTE(Bda->CursorPosition[Page]) * Bda->ScreenColumns +
+                      LOBYTE(Bda->CursorPosition[Page])) * 2;
 
-            if (HIBYTE(Eax) == 0x06) Position.Y = Rect.Top - Lines;
-            else Position.Y = Rect.Top + Lines;
+            if (getAH() == 0x08)
+            {
+                /* Read from the video memory */
+                VgaReadMemory(TO_LINEAR(TEXT_VIDEO_SEG, Offset),
+                              (LPVOID)&CharacterData,
+                              sizeof(WORD));
 
-            ScrollConsoleScreenBuffer(BiosConsoleOutput,
-                                      &Rect,
-                                      &Rect,
-                                      Position,
-                                      &Character);
+                /* Return the character in AX */
+                setAX(CharacterData);
+            }
+            else
+            {
+                /* Write to video memory */
+                VgaWriteMemory(TO_LINEAR(TEXT_VIDEO_SEG, Offset),
+                               (LPVOID)&CharacterData,
+                               (getBH() == 0x09) ? sizeof(WORD) : sizeof(BYTE));
+            }
 
             break;
         }
 
-        /* Read Character And Attribute At Cursor Position */
-        case 0x08:
+        /* Teletype Output */
+        case 0x0E:
         {
-            DWORD Address;
-            
-            /* Make sure this is text mode */
-            if (!VideoModes[CurrentVideoMode].Text) break;
-
-            /* Make sure the selected video page exists */
-            if (HIBYTE(Ebx) >= VideoModes[CurrentVideoMode].Pages) break;
-            
-            /* Find the address */
-            Address = BiosGetVideoMemoryStart()
-                      + HIBYTE(Ebx) * BiosGetVideoPageSize()
-                      + (HIBYTE(Bda->CursorPosition[HIBYTE(Ebx)])
-                      * VideoModes[CurrentVideoMode].Height
-                      + LOBYTE(Bda->CursorPosition[HIBYTE(Ebx)]))
-                      * VideoModes[CurrentVideoMode].Bpp / 8;
-
-            /* Update the video memory at that address */
-            BiosUpdateVideoMemory(Address,
-                                  Address + VideoModes[CurrentVideoMode].Bpp / 8);
-
-            /* Return the result in AX */
-            EmulatorSetRegister(EMULATOR_REG_AX,
-                                *((LPWORD)((ULONG_PTR)BaseAddress + Address)));
+            BiosPrintCharacter(getAL(), getBL(), getBH());
+            break;
+        }
 
+        /* Get Current Video Mode */
+        case 0x0F:
+        {
+            setAX(MAKEWORD(Bda->VideoMode, Bda->ScreenColumns));
+            setBX(MAKEWORD(getBL(), Bda->VideoPage));
             break;
         }
 
-        /* Write Character And Attribute At Cursor Position */
-        case 0x09:
-        case 0x0A:
+        /* Palette Control */
+        case 0x10:
         {
-            BYTE PixelSize = VideoModes[CurrentVideoMode].Bpp / 8;
-            WORD Data = (LOBYTE(Ebx) << 8) | LOBYTE(Eax);
-            WORD Repeat = LOWORD(Ecx);
-            DWORD Address = BiosGetVideoMemoryStart()
-                            + CurrentVideoPage * BiosGetVideoPageSize()
-                            + (HIBYTE(Bda->CursorPosition[CurrentVideoPage])
-                            * VideoModes[CurrentVideoMode].Height
-                            + LOBYTE(Bda->CursorPosition[CurrentVideoPage]))
-                            * PixelSize;
-
-            /* Make sure this is text mode */
-            if (!VideoModes[CurrentVideoMode].Text) break;
+            switch (getAL())
+            {
+                /* Set Single Palette Register */
+                case 0x00:
+                {
+                    /* Write the index */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, getBL());
 
-            /* Make sure the selected video page exists */
-            if (HIBYTE(Ebx) >= VideoModes[CurrentVideoMode].Pages) break;
+                    /* Write the data */
+                    VgaWritePort(VGA_AC_WRITE, getBH());
 
-            /* Make sure we don't write over the end of video memory */
-            Repeat = min(Repeat,
-                        (CONSOLE_VIDEO_MEM_END - Address)
-                        / PixelSize);
+                    /* Enable screen and disable palette access */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, 0x20);
+                    break;
+                }
 
-            /* Copy the values to the memory */
-            for (i = 0; i < Repeat; i++)
-            {
-                if (PixelSize == sizeof(BYTE) || HIBYTE(Eax) == 0x0A)
+                /* Set Overscan Color */
+                case 0x01:
                 {
-                    /* Just characters, no attributes */
-                    *((LPBYTE)((ULONG_PTR)BaseAddress + Address) + i * PixelSize) = LOBYTE(Data);
+                    /* Write the index */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, VGA_AC_OVERSCAN_REG);
+
+                    /* Write the data */
+                    VgaWritePort(VGA_AC_WRITE, getBH());
+
+                    /* Enable screen and disable palette access */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, 0x20);
+                    break;
                 }
-                else if (PixelSize == sizeof(WORD))
+
+                /* Set All Palette Registers */
+                case 0x02:
                 {
-                    /* First byte for characters, second for attributes */
-                    *((LPWORD)((ULONG_PTR)BaseAddress + Address) + i) = Data;
+                    INT i;
+                    LPBYTE Buffer = SEG_OFF_TO_PTR(getES(), getDX());
+
+                    /* Set the palette registers */
+                    for (i = 0; i <= VGA_AC_PAL_F_REG; i++)
+                    {
+                        /* Write the index */
+                        VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                        VgaWritePort(VGA_AC_INDEX, i);
+
+                        /* Write the data */
+                        VgaWritePort(VGA_AC_WRITE, Buffer[i]);
+                    }
+
+                    /* Set the overscan register */
+                    VgaWritePort(VGA_AC_INDEX, VGA_AC_OVERSCAN_REG);
+                    VgaWritePort(VGA_AC_WRITE, Buffer[VGA_AC_PAL_F_REG + 1]);
+
+                    /* Enable screen and disable palette access */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, 0x20);
+                    break;
                 }
-            }
 
-            /* Update the range */
-            BiosUpdateConsole(Address,
-                              Address + Repeat * (VideoModes[CurrentVideoMode].Bpp / 8));
+                /* Get Single Palette Register */
+                case 0x07:
+                {
+                    /* Write the index */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, getBL());
+
+                    /* Read the data */
+                    setBH(VgaReadPort(VGA_AC_READ));
+
+                    /* Enable screen and disable palette access */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, 0x20);
+                    break;
+                }
+
+                /* Get Overscan Color */
+                case 0x08:
+                {
+                    /* Write the index */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, VGA_AC_OVERSCAN_REG);
+
+                    /* Read the data */
+                    setBH(VgaReadPort(VGA_AC_READ));
+
+                    /* Enable screen and disable palette access */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, 0x20);
+                    break;
+                }
+
+                /* Get All Palette Registers */
+                case 0x09:
+                {
+                    INT i;
+                    LPBYTE Buffer = SEG_OFF_TO_PTR(getES(), getDX());
+
+                    /* Get the palette registers */
+                    for (i = 0; i <= VGA_AC_PAL_F_REG; i++)
+                    {
+                        /* Write the index */
+                        VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                        VgaWritePort(VGA_AC_INDEX, i);
+
+                        /* Read the data */
+                        Buffer[i] = VgaReadPort(VGA_AC_READ);
+                    }
+
+                    /* Get the overscan register */
+                    VgaWritePort(VGA_AC_INDEX, VGA_AC_OVERSCAN_REG);
+                    Buffer[VGA_AC_PAL_F_REG + 1] = VgaReadPort(VGA_AC_READ);
+
+                    /* Enable screen and disable palette access */
+                    VgaReadPort(VGA_INSTAT1_READ); // Put the AC register into index state
+                    VgaWritePort(VGA_AC_INDEX, 0x20);
+                    break;
+                }
+
+                /* Set Individual DAC Register */
+                case 0x10:
+                {
+                    /* Write the index */
+                    // Certainly in BL and not in BX as said by Ralf Brown...
+                    VgaWritePort(VGA_DAC_WRITE_INDEX, getBL());
+
+                    /* Write the data in this order: Red, Green, Blue */
+                    VgaWritePort(VGA_DAC_DATA, getDH());
+                    VgaWritePort(VGA_DAC_DATA, getCH());
+                    VgaWritePort(VGA_DAC_DATA, getCL());
+
+                    break;
+                }
+
+                /* Set Block of DAC Registers */
+                case 0x12:
+                {
+                    INT i;
+                    LPBYTE Buffer = SEG_OFF_TO_PTR(getES(), getDX());
+
+                    /* Write the index */
+                    // Certainly in BL and not in BX as said by Ralf Brown...
+                    VgaWritePort(VGA_DAC_WRITE_INDEX, getBL());
+
+                    for (i = 0; i < getCX(); i++)
+                    {
+                        /* Write the data in this order: Red, Green, Blue */
+                        VgaWritePort(VGA_DAC_DATA, *Buffer++);
+                        VgaWritePort(VGA_DAC_DATA, *Buffer++);
+                        VgaWritePort(VGA_DAC_DATA, *Buffer++);
+                    }
+
+                    break;
+                }
+
+                /* Get Individual DAC Register */
+                case 0x15:
+                {
+                    /* Write the index */
+                    VgaWritePort(VGA_DAC_READ_INDEX, getBL());
+
+                    /* Read the data in this order: Red, Green, Blue */
+                    setDH(VgaReadPort(VGA_DAC_DATA));
+                    setCH(VgaReadPort(VGA_DAC_DATA));
+                    setCL(VgaReadPort(VGA_DAC_DATA));
+
+                    break;
+                }
+
+                /* Get Block of DAC Registers */
+                case 0x17:
+                {
+                    INT i;
+                    LPBYTE Buffer = SEG_OFF_TO_PTR(getES(), getDX());
+
+                    /* Write the index */
+                    // Certainly in BL and not in BX as said by Ralf Brown...
+                    VgaWritePort(VGA_DAC_READ_INDEX, getBL());
+
+                    for (i = 0; i < getCX(); i++)
+                    {
+                        /* Write the data in this order: Red, Green, Blue */
+                        *Buffer++ = VgaReadPort(VGA_DAC_DATA);
+                        *Buffer++ = VgaReadPort(VGA_DAC_DATA);
+                        *Buffer++ = VgaReadPort(VGA_DAC_DATA);
+                    }
+
+                    break;
+                }
+
+                default:
+                {
+                    DPRINT1("BIOS Palette Control Sub-command AL = 0x%02X NOT IMPLEMENTED\n",
+                            getAL());
+                    break;
+                }
+            }
 
             break;
         }
 
-        /* Teletype Output */
-        case 0x0E:
+        /* Scroll Window */
+        case 0x12:
         {
-            CHAR Character = LOBYTE(Eax);
-            DWORD NumWritten;
-
-            /* Make sure the page exists */
-            if (HIBYTE(Ebx) >= VideoModes[CurrentVideoMode].Pages) break;
+            SMALL_RECT Rectangle = { getCL(), getCH(), getDL(), getDH() };
 
-            /* Set the attribute */
-            SetConsoleTextAttribute(BiosConsoleOutput, LOBYTE(Ebx));
+            /* Call the internal function */
+            BiosScrollWindow(getBL(),
+                             getAL(),
+                             Rectangle,
+                             Bda->VideoPage,
+                             DEFAULT_ATTRIBUTE);
 
-            /* Write the character */
-            WriteConsoleA(BiosConsoleOutput,
-                          &Character,
-                          sizeof(CHAR),
-                          &NumWritten,
-                          NULL);
+            break;
+        }
 
+        /* Display combination code */
+        case 0x1A:
+        {
+            switch(getAL())
+            {
+                case 0x00: /* Get Display combiantion code */
+                   setAX(MAKEWORD(0x1A, 0x1A));
+                   setBX(MAKEWORD(0x08, 0x00)); /* VGA w/ color analog display */
+                   break;
+                case 0x01: /* Set Display combination code */
+                   DPRINT1("Set Display combination code - Unsupported\n");
+                   break;
+                default:
+                   break;
+            }
             break;
         }
 
-        /* Get Current Video Mode */
-        case 0x0F:
+        default:
+        {
+            DPRINT1("BIOS Function INT 10h, AH = 0x%02X NOT IMPLEMENTED\n",
+                    getAH());
+        }
+    }
+}
+
+VOID WINAPI BiosEquipmentService(LPWORD Stack)
+{
+    /* Return the equipment list */
+    setAX(Bda->EquipmentList);
+}
+
+VOID WINAPI BiosGetMemorySize(LPWORD Stack)
+{
+    /* Return the conventional memory size in kB, typically 640 kB */
+    setAX(Bda->MemorySize);
+}
+
+VOID WINAPI BiosMiscService(LPWORD Stack)
+{
+    switch (getAH())
+    {
+        /* Copy Extended Memory */
+        case 0x87:
         {
-            EmulatorSetRegister(EMULATOR_REG_AX,
-                                MAKEWORD(Bda->VideoMode, Bda->ScreenColumns));
-            EmulatorSetRegister(EMULATOR_REG_BX,
-                                MAKEWORD(LOBYTE(Ebx), Bda->VideoPage));
+            DWORD Count = (DWORD)getCX() * 2;
+            PFAST486_GDT_ENTRY Gdt = (PFAST486_GDT_ENTRY)SEG_OFF_TO_PTR(getES(), getSI());
+            DWORD SourceBase = Gdt[2].Base + (Gdt[2].BaseMid << 16) + (Gdt[2].BaseHigh << 24);
+            DWORD SourceLimit = Gdt[2].Limit + (Gdt[2].LimitHigh << 16);
+            DWORD DestBase = Gdt[3].Base + (Gdt[3].BaseMid << 16) + (Gdt[3].BaseHigh << 24);
+            DWORD DestLimit = Gdt[3].Limit + (Gdt[3].LimitHigh << 16);
+
+            /* Check for flags */
+            if (Gdt[2].Granularity) SourceLimit = (SourceLimit << 12) | 0xFFF;
+            if (Gdt[3].Granularity) DestLimit = (DestLimit << 12) | 0xFFF;
+
+            if ((Count > SourceLimit) || (Count > DestLimit))
+            {
+                setAX(0x80);
+                Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
+
+                break;
+            }
+
+            /* Copy */
+            RtlMoveMemory((PVOID)((ULONG_PTR)BaseAddress + DestBase),
+                          (PVOID)((ULONG_PTR)BaseAddress + SourceBase),
+                          Count);
 
+            setAX(ERROR_SUCCESS);
+            Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
             break;
         }
 
-        /* Scroll Window */
-        case 0x12:
+        /* Get Extended Memory Size */
+        case 0x88:
         {
-            Rect.Top = HIBYTE(Ecx);
-            Rect.Left = LOBYTE(Ecx);
-            Rect.Bottom = HIBYTE(Edx);
-            Rect.Right = LOBYTE(Edx);
-            Character.Char.UnicodeChar = L' ';
-            Character.Attributes = 0x07;
-            Position.X = Rect.Left;
-            Position.Y = Rect.Top;
-
-            if (LOBYTE(Ebx) == 0) Position.Y -= LOBYTE(Eax);
-            else if (LOBYTE(Ebx) == 1) Position.Y += LOBYTE(Eax);
-            else if (LOBYTE(Ebx) == 2) Position.X -= LOBYTE(Eax);
-            else if (LOBYTE(Ebx) == 3) Position.X += LOBYTE(Eax);
-
-            ScrollConsoleScreenBuffer(BiosConsoleOutput,
-                                      &Rect,
-                                      &Rect,
-                                      Position,
-                                      &Character);
+            /* Return the number of KB of RAM after 1 MB */
+            setAX((MAX_ADDRESS - 0x100000) / 1024);
+
+            /* Clear CF */
+            Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
 
             break;
         }
 
         default:
         {
-            DPRINT1("BIOS Function INT 10h, AH = 0x%02X NOT IMPLEMENTED\n",
-                    HIBYTE(Eax));
+            DPRINT1("BIOS Function INT 15h, AH = 0x%02X NOT IMPLEMENTED\n",
+                    getAH());
         }
     }
 }
 
-VOID BiosKeyboardService()
+VOID WINAPI BiosKeyboardService(LPWORD Stack)
 {
-    DWORD Eax = EmulatorGetRegister(EMULATOR_REG_AX);
-
-    switch (HIBYTE(Eax))
+    switch (getAH())
     {
+        /* Wait for keystroke and read */
         case 0x00:
+        /* Wait for extended keystroke and read */
+        case 0x10:  // FIXME: Temporarily do the same as INT 16h, 00h
         {
             /* Read the character (and wait if necessary) */
-            EmulatorSetRegister(EMULATOR_REG_AX, BiosGetCharacter());
-
+            setAX(BiosGetCharacter());
             break;
         }
 
+        /* Get keystroke status */
         case 0x01:
+        /* Get extended keystroke status */
+        case 0x11:  // FIXME: Temporarily do the same as INT 16h, 01h
         {
             WORD Data = BiosPeekCharacter();
 
             if (Data != 0xFFFF)
             {
                 /* There is a character, clear ZF and return it */
-                EmulatorSetRegister(EMULATOR_REG_AX, Data);
-                EmulatorClearFlag(EMULATOR_FLAG_ZF);
+                Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_ZF;
+                setAX(Data);
             }
             else
             {
                 /* No character, set ZF */
-                EmulatorSetFlag(EMULATOR_FLAG_ZF);
+                Stack[STACK_FLAGS] |= EMULATOR_FLAG_ZF;
             }
 
             break;
         }
-        
+
+        /* Get shift status */
+        case 0x02:
+        {
+            /* Return the lower byte of the keyboard shift status word */
+            setAL(LOBYTE(Bda->KeybdShiftFlags));
+            break;
+        }
+
+        /* Reserved */
+        case 0x04:
+        {
+            DPRINT1("BIOS Function INT 16h, AH = 0x04 is RESERVED\n");
+            break;
+        }
+
+        /* Push keystroke */
+        case 0x05:
+        {
+            /* Return 0 if success, 1 if failure */
+            setAL(BiosKbdBufferPush(getCX()) == FALSE);
+            break;
+        }
+
+        /* Get extended shift status */
+        case 0x12:
+        {
+            /*
+             * Be careful! The returned word is similar to Bda->KeybdShiftFlags
+             * but the high byte is organized differently:
+             * the bytes 2 and 3 of the high byte are not the same...
+             */
+            WORD KeybdShiftFlags = (Bda->KeybdShiftFlags & 0xF3FF);
+
+            /* Return the extended keyboard shift status word */
+            setAX(KeybdShiftFlags);
+            break;
+        }
+
         default:
         {
             DPRINT1("BIOS Function INT 16h, AH = 0x%02X NOT IMPLEMENTED\n",
-                    HIBYTE(Eax));
+                    getAH());
         }
     }
 }
 
-VOID BiosTimeService()
+VOID WINAPI BiosTimeService(LPWORD Stack)
 {
-    DWORD Eax = EmulatorGetRegister(EMULATOR_REG_AX);
-    DWORD Ecx = EmulatorGetRegister(EMULATOR_REG_CX);
-    DWORD Edx = EmulatorGetRegister(EMULATOR_REG_DX);
-
-    switch (HIBYTE(Eax))
+    switch (getAH())
     {
         case 0x00:
         {
             /* Set AL to 1 if midnight had passed, 0 otherwise */
-            Eax &= 0xFFFFFF00;
-            if (Bda->MidnightPassed) Eax |= 1;
+            setAL(Bda->MidnightPassed ? 0x01 : 0x00);
 
             /* Return the tick count in CX:DX */
-            EmulatorSetRegister(EMULATOR_REG_AX, Eax);
-            EmulatorSetRegister(EMULATOR_REG_CX, HIWORD(Bda->TickCounter));
-            EmulatorSetRegister(EMULATOR_REG_DX, LOWORD(Bda->TickCounter));
+            setCX(HIWORD(Bda->TickCounter));
+            setDX(LOWORD(Bda->TickCounter));
 
             /* Reset the midnight flag */
             Bda->MidnightPassed = FALSE;
@@ -997,7 +1515,7 @@ VOID BiosTimeService()
         case 0x01:
         {
             /* Set the tick count to CX:DX */
-            Bda->TickCounter = MAKELONG(LOWORD(Edx), LOWORD(Ecx));
+            Bda->TickCounter = MAKELONG(getDX(), getCX());
 
             /* Reset the midnight flag */
             Bda->MidnightPassed = FALSE;
@@ -1008,24 +1526,18 @@ VOID BiosTimeService()
         default:
         {
             DPRINT1("BIOS Function INT 1Ah, AH = 0x%02X NOT IMPLEMENTED\n",
-                    HIBYTE(Eax));
+                    getAH());
         }
     }
 }
 
-VOID BiosSystemTimerInterrupt()
+VOID WINAPI BiosSystemTimerInterrupt(LPWORD Stack)
 {
     /* Increase the system tick count */
     Bda->TickCounter++;
 }
 
-VOID BiosEquipmentService()
-{
-    /* Return the equipment list */
-    EmulatorSetRegister(EMULATOR_REG_AX, Bda->EquipmentList);
-}
-
-VOID BiosHandleIrq(BYTE IrqNumber)
+VOID BiosHandleIrq(BYTE IrqNumber, LPWORD Stack)
 {
     switch (IrqNumber)
     {
@@ -1034,7 +1546,6 @@ VOID BiosHandleIrq(BYTE IrqNumber)
         {
             /* Perform the system timer interrupt */
             EmulatorInterrupt(BIOS_SYS_TIMER_INTERRUPT);
-
             break;
         }
 
@@ -1043,21 +1554,19 @@ VOID BiosHandleIrq(BYTE IrqNumber)
         {
             BYTE ScanCode, VirtualKey;
             WORD Character;
-            
-            /* Check if there is a scancode available */
-            if (!(KeyboardReadStatus() & 1)) break;
 
             /* Get the scan code and virtual key code */
-            ScanCode = KeyboardReadData();
+            ScanCode = PS2ReadPort(PS2_DATA_PORT);
             VirtualKey = MapVirtualKey(ScanCode & 0x7F, MAPVK_VSC_TO_VK);
 
             /* Check if this is a key press or release */
             if (!(ScanCode & (1 << 7)))
             {
                 /* Key press */
-                if (VirtualKey == VK_NUMLOCK
-                    || VirtualKey == VK_CAPITAL
-                    || VirtualKey == VK_SCROLL)
+                if (VirtualKey == VK_NUMLOCK ||
+                    VirtualKey == VK_CAPITAL ||
+                    VirtualKey == VK_SCROLL  ||
+                    VirtualKey == VK_INSERT)
                 {
                     /* For toggle keys, toggle the lowest bit in the keyboard map */
                     BiosKeyboardMap[VirtualKey] ^= ~(1 << 0);
@@ -1067,11 +1576,15 @@ VOID BiosHandleIrq(BYTE IrqNumber)
                 BiosKeyboardMap[VirtualKey] |= (1 << 7);
 
                 /* Find out which character this is */
-                if (ToAscii(VirtualKey, ScanCode, BiosKeyboardMap, &Character, 0) > 0)
+                Character = 0;
+                if (ToAscii(VirtualKey, ScanCode, BiosKeyboardMap, &Character, 0) == 0)
                 {
-                    /* Push it onto the BIOS keyboard queue */
-                    BiosKbdBufferPush((ScanCode << 8) | (Character & 0xFF));
+                    /* Not ASCII */
+                    Character = 0;
                 }
+
+                /* Push it onto the BIOS keyboard queue */
+                BiosKbdBufferPush(MAKEWORD(Character, ScanCode));
             }
             else
             {
@@ -1079,12 +1592,33 @@ VOID BiosHandleIrq(BYTE IrqNumber)
                 BiosKeyboardMap[VirtualKey] &= ~(1 << 7);
             }
 
+            /* Clear the keyboard flags */
+            Bda->KeybdShiftFlags = 0;
+
+            /* Set the appropriate flags based on the state */
+            if (BiosKeyboardMap[VK_RSHIFT]   & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_RSHIFT;
+            if (BiosKeyboardMap[VK_LSHIFT]   & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_LSHIFT;
+            if (BiosKeyboardMap[VK_CONTROL]  & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_CTRL;
+            if (BiosKeyboardMap[VK_MENU]     & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_ALT;
+            if (BiosKeyboardMap[VK_SCROLL]   & (1 << 0)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_SCROLL_ON;
+            if (BiosKeyboardMap[VK_NUMLOCK]  & (1 << 0)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_NUMLOCK_ON;
+            if (BiosKeyboardMap[VK_CAPITAL]  & (1 << 0)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_CAPSLOCK_ON;
+            if (BiosKeyboardMap[VK_INSERT]   & (1 << 0)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_INSERT_ON;
+            if (BiosKeyboardMap[VK_RMENU]    & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_RALT;
+            if (BiosKeyboardMap[VK_LMENU]    & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_LALT;
+            if (BiosKeyboardMap[VK_SNAPSHOT] & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_SYSRQ;
+            if (BiosKeyboardMap[VK_PAUSE]    & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_PAUSE;
+            if (BiosKeyboardMap[VK_SCROLL]   & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_SCROLL;
+            if (BiosKeyboardMap[VK_NUMLOCK]  & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_NUMLOCK;
+            if (BiosKeyboardMap[VK_CAPITAL]  & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_CAPSLOCK;
+            if (BiosKeyboardMap[VK_INSERT]   & (1 << 7)) Bda->KeybdShiftFlags |= BDA_KBDFLAG_INSERT;
+
             break;
         }
     }
 
     /* Send End-of-Interrupt to the PIC */
-    if (IrqNumber > 8) PicWriteCommand(PIC_SLAVE_CMD, PIC_OCW2_EOI);
+    if (IrqNumber >= 8) PicWriteCommand(PIC_SLAVE_CMD, PIC_OCW2_EOI);
     PicWriteCommand(PIC_MASTER_CMD, PIC_OCW2_EOI);
 }