[NTVDM]: Add a DPRINT1 that can be useful later on...
[reactos.git] / subsystems / ntvdm / bios.c
index 37e84df..c2807f4 100644 (file)
 #include "ps2.h"
 #include "timer.h"
 
+#include "registers.h"
+
 /* PRIVATE VARIABLES **********************************************************/
 
-static PBIOS_DATA_AREA Bda;
+PBIOS_DATA_AREA Bda;
 static BYTE BiosKeyboardMap[256];
 static HANDLE BiosConsoleInput  = INVALID_HANDLE_VALUE;
 static HANDLE BiosConsoleOutput = INVALID_HANDLE_VALUE;
 static CONSOLE_SCREEN_BUFFER_INFO BiosSavedBufferInfo;
 
+/*
+ * VGA Register Configurations for BIOS Video Modes
+ * The configurations come from DosBox.
+ */
 static BYTE VideoMode_40x25_text[] =
 {
     /* Miscellaneous Register */
     0x67,
 
     /* Sequencer Registers */
-    0x03, 0x08, 0x03, 0x00, 0x02,
+    0x00, 0x08, 0x03, 0x00, 0x07,
 
     /* GC Registers */
     0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0E, 0x0F, 0xFF,
 
     /* CRTC Registers */
-    0x2D, 0x27, 0x28, 0x90, 0x2B, 0xA0, 0xBF, 0x1F, 0x00, 0x4F, 0x0D, 0x0F,
+    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,
 
     /* AC Registers */
     0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
-    0x3C, 0x3D, 0x3E, 0x3F, 0x0C, 0x01, 0x0F, 0x13, 0x00
+    0x3C, 0x3D, 0x3E, 0x3F, 0x0C, 0x00, 0x0F, 0x08, 0x00
 };
 
 static BYTE VideoMode_80x25_text[] =
@@ -52,19 +58,19 @@ static BYTE VideoMode_80x25_text[] =
     0x67,
 
     /* Sequencer Registers */
-    0x03, 0x00, 0x03, 0x00, 0x02,
+    0x00, 0x00, 0x03, 0x00, 0x07,
 
     /* GC Registers */
     0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0E, 0x0F, 0xFF,
 
     /* CRTC Registers */
-    0x5F, 0x4F, 0x50, 0x82, 0x55, 0x81, 0xBF, 0x1F, 0x00, 0x4F, 0x0D, 0x0F,
+    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,
 
     /* AC Registers */
     0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
-    0x3C, 0x3D, 0x3E, 0x3F, 0x0C, 0x01, 0x0F, 0x13, 0x00
+    0x3C, 0x3D, 0x3E, 0x3F, 0x0C, 0x00, 0x0F, 0x08, 0x00
 };
 
 static BYTE VideoMode_320x200_4color[] =
@@ -73,10 +79,10 @@ static BYTE VideoMode_320x200_4color[] =
     0x63,
 
     /* Sequencer Registers */
-    0x03, 0x09, 0x03, 0x00, 0x02,
+    0x00, 0x09, 0x00, 0x00, 0x02,
 
     /* GC Registers */
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x0F, 0x0F, 0xFF,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0F, 0x0F, 0xFF,
 
     /* CRTC Registers */
     0x2D, 0x27, 0x28, 0x90, 0x2B, 0x80, 0xBF, 0x1F, 0x00, 0xC1, 0x00, 0x00,
@@ -85,7 +91,112 @@ static BYTE VideoMode_320x200_4color[] =
 
     /* AC Registers */
     0x00, 0x13, 0x15, 0x17, 0x02, 0x04, 0x06, 0x07, 0x10, 0x11, 0x12, 0x13,
-    0x14, 0x15, 0x16, 0x17, 0x01, 0x00, 0x03, 0x00, 0x00
+    0x14, 0x15, 0x16, 0x17, 0x01, 0x00, 0x0F, 0x00, 0x00
+};
+
+static BYTE VideoMode_640x200_2color[] =
+{
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    0x00, 0x09, 0x0F, 0x00, 0x02,
+
+    /* GC Registers */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0xFF,
+
+    /* 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,
+
+    /* AC Registers */
+    0x00, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17,
+    0x17, 0x17, 0x17, 0x17, 0x01, 0x00, 0x01, 0x00, 0x00
+};
+
+static BYTE VideoMode_320x200_16color[] =
+{
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    0x00, 0x09, 0x0F, 0x00, 0x02,
+
+    /* GC Registers */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF,
+
+    /* 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,
+
+    /* AC Registers */
+    0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x10, 0x11, 0x12, 0x13,
+    0x14, 0x15, 0x16, 0x17, 0x01, 0x00, 0x0F, 0x00, 0x00
+};
+
+static BYTE VideoMode_640x200_16color[] =
+{
+    /* Miscellaneous Register */
+    0x63,
+
+    /* Sequencer Registers */
+    0x00, 0x01, 0x0F, 0x00, 0x02,
+
+    /* GC Registers */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF,
+
+    /* 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,
+
+    /* AC Registers */
+    0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x10, 0x11, 0x12, 0x13,
+    0x14, 0x15, 0x16, 0x17, 0x01, 0x00, 0x0F, 0x00, 0x00
+};
+
+static BYTE VideoMode_640x350_16color[] =
+{
+    /* Miscellaneous Register */
+    0xA3,
+
+    /* Sequencer Registers */
+    0x00, 0x01, 0x0F, 0x00, 0x02,
+
+    /* GC Registers */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF,
+
+    /* 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,
+
+    /* AC Registers */
+    0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
+    0x3C, 0x3D, 0x3E, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00
+};
+
+static BYTE VideoMode_640x480_2color[] =
+{
+    /* Miscellaneous Register */
+    0xE3,
+
+    /* Sequencer Registers */
+    0x00, 0x01, 0x0F, 0x00, 0x02,
+
+    /* GC Registers */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF,
+
+    /* 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,
+
+    /* AC Registers */
+    0x00, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+    0x3F, 0x3F, 0x3F, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00
 };
 
 static BYTE VideoMode_640x480_16color[] =
@@ -94,14 +205,14 @@ static BYTE VideoMode_640x480_16color[] =
     0xE3,
 
     /* Sequencer Registers */
-    0x03, 0x01, 0x08, 0x00, 0x06,
+    0x00, 0x01, 0x0F, 0x00, 0x02,
 
     /* GC Registers */
-    0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x05, 0x0F, 0xFF,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0F, 0xFF,
 
     /* CRTC Registers */
     0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0x0B, 0x3E, 0x00, 0x40, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0xEA, 0x0C, 0xDF, 0x28, 0x00, 0xE7, 0x04, 0xE3,
+    0x00, 0x00, 0x00, 0x00, 0xEA, 0x8C, 0xDF, 0x28, 0x00, 0xE7, 0x04, 0xE3,
     0xFF,
 
     /* AC Registers */
@@ -115,14 +226,14 @@ static BYTE VideoMode_320x200_256color[] =
     0x63,
 
     /* Sequencer Registers */
-    0x03, 0x01, 0x0F, 0x00, 0x0E,
+    0x00, 0x01, 0x0F, 0x00, 0x0E,
 
     /* GC Registers */
     0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F, 0xFF,
 
     /* CRTC Registers */
     0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0xBF, 0x1F, 0x00, 0x41, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x9C, 0x0E, 0x8F, 0x28, 0x40, 0x96, 0xB9, 0xA3,
+    0x00, 0x00, 0x00, 0x00, 0x9C, 0x8E, 0x8F, 0x28, 0x40, 0x96, 0xB9, 0xA3,
     0xFF,
 
     /* AC Registers */
@@ -138,18 +249,18 @@ static LPBYTE VideoModes[] =
     VideoMode_80x25_text,       /* Mode 03h */
     VideoMode_320x200_4color,   /* Mode 04h */
     VideoMode_320x200_4color,   /* Mode 05h */
-    NULL,                       /* Mode 06h */
+    VideoMode_640x200_2color,   /* Mode 06h */
     NULL,                       /* Mode 07h */
     NULL,                       /* Mode 08h */
     NULL,                       /* Mode 09h */
     NULL,                       /* Mode 0Ah */
     NULL,                       /* Mode 0Bh */
     NULL,                       /* Mode 0Ch */
-    NULL,                       /* Mode 0Dh */
-    NULL,                       /* Mode 0Eh */
+    VideoMode_320x200_16color,  /* Mode 0Dh */
+    VideoMode_640x200_16color,  /* Mode 0Eh */
     NULL,                       /* Mode 0Fh */
-    NULL,                       /* Mode 10h */
-    NULL,                       /* Mode 11h */
+    VideoMode_640x350_16color,  /* Mode 10h */
+    VideoMode_640x480_2color,   /* Mode 11h */
     VideoMode_640x480_16color,  /* Mode 12h */
     VideoMode_320x200_256color, /* Mode 13h */
 };
@@ -158,14 +269,14 @@ static LPBYTE VideoModes[] =
 
 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;
@@ -247,7 +358,7 @@ static VOID BiosWriteWindow(LPWORD Buffer, SMALL_RECT Rectangle, BYTE Page)
         {
             Character = Buffer[Counter++];
 
-            /* Read from video memory */
+            /* Write to video memory */
             VgaWriteMemory(VideoAddress + (i * Bda->ScreenColumns + j) * sizeof(WORD),
                            (LPVOID)&Character,
                            sizeof(WORD));
@@ -268,6 +379,8 @@ BOOLEAN BiosSetVideoMode(BYTE ModeNumber)
     COORD Resolution;
     LPBYTE Values = VideoModes[ModeNumber];
 
+    DPRINT1("Switching to mode %Xh; Values = 0x%p\n", ModeNumber, Values);
+
     if (Values == NULL) return FALSE;
 
     /* Write the misc register */
@@ -304,7 +417,7 @@ BOOLEAN BiosSetVideoMode(BYTE ModeNumber)
     /* Update the values in the BDA */
     Bda->VideoMode = ModeNumber;
     Bda->VideoPage = 0;
-    Bda->VideoPageSize   = BIOS_PAGE_SIZE;
+    Bda->VideoPageSize = BIOS_PAGE_SIZE;
     Bda->VideoPageOffset = 0;
 
     /* Get the character height */
@@ -320,8 +433,12 @@ BOOLEAN BiosSetVideoMode(BYTE ModeNumber)
 
 BOOLEAN BiosSetVideoPage(BYTE PageNumber)
 {
+    /* Check if the page exists */
     if (PageNumber >= BIOS_MAX_PAGES) return FALSE;
 
+    /* Check if this is the same page */
+    if (PageNumber == Bda->VideoPage) return TRUE;
+
     /* Set the values in the BDA */
     Bda->VideoPage = PageNumber;
     Bda->VideoPageSize = BIOS_PAGE_SIZE;
@@ -338,36 +455,51 @@ BOOLEAN BiosSetVideoPage(BYTE PageNumber)
 
 BOOLEAN BiosInitialize(VOID)
 {
-    INT i;
+    USHORT i;
     WORD Offset = 0;
-    LPWORD IntVecTable = (LPWORD)((ULONG_PTR)BaseAddress);
-    LPBYTE BiosCode = (LPBYTE)((ULONG_PTR)BaseAddress + TO_LINEAR(BIOS_SEGMENT, 0));
+    LPWORD IntVecTable = (LPWORD)BaseAddress;
+    LPBYTE BiosCode = (LPBYTE)SEG_OFF_TO_PTR(BIOS_SEGMENT, 0);
 
     /* Initialize the BDA */
-    Bda = (PBIOS_DATA_AREA)((ULONG_PTR)BaseAddress + TO_LINEAR(BDA_SEGMENT, 0));
+    Bda = (PBIOS_DATA_AREA)SEG_OFF_TO_PTR(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);
 
     /* Generate ISR stubs and fill the IVT */
-    for (i = 0; i < 256; i++)
+    for (i = 0x00; i <= 0xFF; i++)
     {
         IntVecTable[i * 2] = Offset;
         IntVecTable[i * 2 + 1] = BIOS_SEGMENT;
 
-        BiosCode[Offset++] = 0xFA; // cli
+        BiosCode[Offset++] = 0xFB; // sti
 
         BiosCode[Offset++] = 0x6A; // push i
-        BiosCode[Offset++] = (BYTE)i;
+        BiosCode[Offset++] = (UCHAR)i;
+
+        BiosCode[Offset++] = 0x6A; // push 0
+        BiosCode[Offset++] = 0x00;
+
+// BOP_SEQ:
+        BiosCode[Offset++] = 0xF8; // clc
 
         BiosCode[Offset++] = LOBYTE(EMULATOR_BOP); // BOP sequence
         BiosCode[Offset++] = HIBYTE(EMULATOR_BOP);
-        BiosCode[Offset++] = LOBYTE(EMULATOR_INT_BOP);
-        BiosCode[Offset++] = HIBYTE(EMULATOR_INT_BOP);
+        BiosCode[Offset++] = EMULATOR_INT_BOP;
+
+        BiosCode[Offset++] = 0x73; // jnc EXIT (offset +3)
+        BiosCode[Offset++] = 0x03;
 
-        BiosCode[Offset++] = 0x83; // add sp, 2
+        // HACK: The following instruction should be HLT!
+        BiosCode[Offset++] = 0x90; // nop
+
+        BiosCode[Offset++] = 0xEB; // jmp BOP_SEQ (offset -9)
+        BiosCode[Offset++] = 0xF7;
+
+// EXIT:
+        BiosCode[Offset++] = 0x83; // add sp, 4
         BiosCode[Offset++] = 0xC4;
-        BiosCode[Offset++] = 0x02;
+        BiosCode[Offset++] = 0x04;
 
         BiosCode[Offset++] = 0xCF; // iret
     }
@@ -408,7 +540,12 @@ BOOLEAN BiosInitialize(VOID)
     }
 
     /* Initialize VGA */
-    VgaInitialize(BiosConsoleOutput);
+    if (!VgaInitialize(BiosConsoleOutput))
+    {
+        CloseHandle(BiosConsoleOutput);
+        CloseHandle(BiosConsoleInput);
+        return FALSE;
+    }
 
     /* Update the cursor position */
     BiosSetCursorPosition(BiosSavedBufferInfo.dwCursorPosition.Y,
@@ -460,22 +597,16 @@ VOID BiosCleanup(VOID)
 
 WORD BiosPeekCharacter(VOID)
 {
-    WORD CharacterData;
-    
-    /* Check if there is a key available */
-    if (Bda->KeybdBufferHead == Bda->KeybdBufferTail) return 0xFFFF;
+    WORD CharacterData = 0;
 
     /* Get the key from the queue, but don't remove it */
-    BiosKbdBufferTop(&CharacterData);
-
-    return CharacterData;
+    if (BiosKbdBufferTop(&CharacterData)) return CharacterData;
+    else return 0xFFFF;
 }
 
 WORD BiosGetCharacter(VOID)
 {
-    WORD CharacterData;
-    INPUT_RECORD InputRecord;
-    DWORD Count;
+    WORD CharacterData = 0;
 
     /* Check if there is a key available */
     if (Bda->KeybdBufferHead != Bda->KeybdBufferTail)
@@ -486,22 +617,8 @@ WORD BiosGetCharacter(VOID)
     }
     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;
-
-            break;
-        }
+        /* Set the handler CF to repeat the BOP */
+        EmulatorSetFlag(EMULATOR_FLAG_CF);
     }
 
     return CharacterData;
@@ -583,33 +700,78 @@ VOID BiosPrintCharacter(CHAR Character, BYTE Attribute, BYTE Page)
     Row = HIBYTE(Bda->CursorPosition[Page]);
     Column = LOBYTE(Bda->CursorPosition[Page]);
 
-    /* Write the character */
-    VgaWriteMemory(TO_LINEAR(TEXT_VIDEO_SEG,
-                   (Row * Bda->ScreenColumns + Column) * sizeof(WORD)),
-                   (LPVOID)&CharData,
-                   sizeof(WORD));
+    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--;
+        }
+
+        /* Erase the existing character */
+        CharData = (Attribute << 8) | ' ';
+        VgaWriteMemory(TO_LINEAR(TEXT_VIDEO_SEG,
+                       Page * Bda->VideoPageSize
+                       + (Row * Bda->ScreenColumns + Column) * sizeof(WORD)),
+                       (LPVOID)&CharData,
+                       sizeof(WORD));
+    }
+    else if (Character == '\n')
+    {
+        /* Line Feed control character */
+        Row++;
+    }
+    else if (Character == '\r')
+    {
+        /* Carriage Return control character */
+        Column = 0;
+    }
+    else
+    {
+        /* Default character */
+
+        /* Write the character */
+        VgaWriteMemory(TO_LINEAR(TEXT_VIDEO_SEG,
+                       Page * Bda->VideoPageSize
+                       + (Row * Bda->ScreenColumns + Column) * sizeof(WORD)),
+                       (LPVOID)&CharData,
+                       sizeof(WORD));
 
-    /* Advance the cursor */
-    Column++;
+        /* Advance the cursor */
+        Column++;
+    }
 
     /* Check if it passed the end of the row */
-    if (Column == Bda->ScreenColumns)
+    if (Column >= Bda->ScreenColumns)
     {
-        /* Return to the first column */
+        /* Return to the first column and go to the next line */
         Column = 0;
+        Row++;
+    }
 
-        if (Row == Bda->ScreenRows)
-        {
-            /* The screen must be scrolled */
-            SMALL_RECT Rectangle = { 0, 0, Bda->ScreenColumns - 1, Bda->ScreenRows };
-
-            BiosScrollWindow(SCROLL_DIRECTION_UP,
-                             1,
-                             Rectangle,
-                             Page,
-                             DEFAULT_ATTRIBUTE);
-        }
-        else Row++;
+    /* 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);
     }
 
     /* Set the cursor position */
@@ -618,17 +780,13 @@ VOID BiosPrintCharacter(CHAR Character, BYTE Attribute, BYTE Page)
 
 VOID BiosVideoService(LPWORD Stack)
 {
-    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;
         }
 
@@ -636,8 +794,8 @@ VOID BiosVideoService(LPWORD Stack)
         case 0x01:
         {
             /* Update the BDA */
-            Bda->CursorStartLine = HIBYTE(Ecx);
-            Bda->CursorEndLine = LOBYTE(Ecx);
+            Bda->CursorStartLine = getCH();
+            Bda->CursorEndLine   = getCL();
 
             /* Modify the CRTC registers */
             VgaWritePort(VGA_CRTC_INDEX, VGA_CRTC_CURSOR_START_REG);
@@ -651,7 +809,7 @@ VOID BiosVideoService(LPWORD Stack)
         /* Set Cursor Position */
         case 0x02:
         {
-            BiosSetCursorPosition(HIBYTE(Edx), LOBYTE(Edx), HIBYTE(Ebx));
+            BiosSetCursorPosition(getDH(), getDL(), getBH());
             break;
         }
 
@@ -659,14 +817,12 @@ VOID BiosVideoService(LPWORD Stack)
         case 0x03:
         {
             /* Make sure the selected video page exists */
-            if (HIBYTE(Ebx) >= BIOS_MAX_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;
         }
 
@@ -677,22 +833,14 @@ VOID BiosVideoService(LPWORD Stack)
              * On modern BIOSes, this function returns 0
              * so that we can ignore the other registers.
              */
-            EmulatorSetRegister(EMULATOR_REG_AX, 0);
+            setAX(0);
             break;
         }
 
         /* Select Active Display Page */
         case 0x05:
         {
-            /* Check if the page exists */
-            if (LOBYTE(Eax) >= BIOS_MAX_PAGES) break;
-
-            /* Check if this is the same page */
-            if (LOBYTE(Eax) == Bda->VideoPage) break;
-
-            /* Change the video page */
-            BiosSetVideoPage(LOBYTE(Eax));
-
+            BiosSetVideoPage(getAL());
             break;
         }
 
@@ -700,21 +848,15 @@ VOID BiosVideoService(LPWORD Stack)
         case 0x06:
         case 0x07:
         {
-            SMALL_RECT Rectangle =
-            {
-                LOBYTE(Ecx),
-                HIBYTE(Ecx),
-                LOBYTE(Edx),
-                HIBYTE(Edx)
-            };
+            SMALL_RECT Rectangle = { getCL(), getCH(), getDL(), getDH() };
 
             /* Call the internal function */
-            BiosScrollWindow((HIBYTE(Eax)== 0x06)
-                             ? SCROLL_DIRECTION_UP : SCROLL_DIRECTION_DOWN,
-                             LOBYTE(Eax),
+            BiosScrollWindow((getAH() == 0x06) ? SCROLL_DIRECTION_UP
+                                               : SCROLL_DIRECTION_DOWN,
+                             getAL(),
                              Rectangle,
                              Bda->VideoPage,
-                             HIBYTE(Ebx));
+                             getBH());
 
             break;
         }
@@ -724,19 +866,19 @@ VOID BiosVideoService(LPWORD Stack)
         case 0x09:
         case 0x0A:
         {
-            WORD CharacterData = MAKEWORD(LOBYTE(Eax), LOBYTE(Ebx));
-            BYTE Page = HIBYTE(Ebx);
+            WORD CharacterData = MAKEWORD(getAL(), getBL());
+            BYTE Page = getBH();
             DWORD Offset;
 
             /* Check if the page exists */
             if (Page >= BIOS_MAX_PAGES) break;
 
             /* Find the offset of the character */
-            Offset = Page * Bda->VideoPageSize
-                     + (HIBYTE(Bda->CursorPosition[Page]) * Bda->ScreenColumns
-                     + LOBYTE(Bda->CursorPosition[Page])) * 2;
+            Offset = Page * Bda->VideoPageSize +
+                     (HIBYTE(Bda->CursorPosition[Page]) * Bda->ScreenColumns +
+                      LOBYTE(Bda->CursorPosition[Page])) * 2;
 
-            if (HIBYTE(Eax) == 0x08)
+            if (getAH() == 0x08)
             {
                 /* Read from the video memory */
                 VgaReadMemory(TO_LINEAR(TEXT_VIDEO_SEG, Offset),
@@ -744,14 +886,14 @@ VOID BiosVideoService(LPWORD Stack)
                               sizeof(WORD));
 
                 /* Return the character in AX */
-                EmulatorSetRegister(EMULATOR_REG_AX, CharacterData);
+                setAX(CharacterData);
             }
             else
             {
                 /* Write to video memory */
                 VgaWriteMemory(TO_LINEAR(TEXT_VIDEO_SEG, Offset),
                                (LPVOID)&CharacterData,
-                               (HIBYTE(Ebx) == 0x09) ? sizeof(WORD) : sizeof(BYTE));
+                               (getBH() == 0x09) ? sizeof(WORD) : sizeof(BYTE));
             }
 
             break;
@@ -760,35 +902,26 @@ VOID BiosVideoService(LPWORD Stack)
         /* Teletype Output */
         case 0x0E:
         {
-            BiosPrintCharacter(LOBYTE(Eax), LOBYTE(Ebx), HIBYTE(Ebx));
+            BiosPrintCharacter(getAL(), getBL(), getBH());
             break;
         }
 
         /* Get Current Video Mode */
         case 0x0F:
         {
-            EmulatorSetRegister(EMULATOR_REG_AX,
-                                MAKEWORD(Bda->VideoMode, Bda->ScreenColumns));
-            EmulatorSetRegister(EMULATOR_REG_BX,
-                                MAKEWORD(LOBYTE(Ebx), Bda->VideoPage));
-
+            setAX(MAKEWORD(Bda->VideoMode, Bda->ScreenColumns));
+            setBX(MAKEWORD(getBL(), Bda->VideoPage));
             break;
         }
 
         /* Scroll Window */
         case 0x12:
         {
-            SMALL_RECT Rectangle =
-            {
-                LOBYTE(Ecx),
-                HIBYTE(Ecx),
-                LOBYTE(Edx),
-                HIBYTE(Edx)
-            };
+            SMALL_RECT Rectangle = { getCL(), getCH(), getDL(), getDH() };
 
             /* Call the internal function */
-            BiosScrollWindow(LOBYTE(Ebx),
-                             LOBYTE(Eax),
+            BiosScrollWindow(getBL(),
+                             getAL(),
                              Rectangle,
                              Bda->VideoPage,
                              DEFAULT_ATTRIBUTE);
@@ -796,37 +929,58 @@ VOID BiosVideoService(LPWORD Stack)
             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;
+        }
+
         default:
         {
             DPRINT1("BIOS Function INT 10h, AH = 0x%02X NOT IMPLEMENTED\n",
-                    HIBYTE(Eax));
+                    getAH());
         }
     }
 }
 
 VOID 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);
                 Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_ZF;
+                setAX(Data);
             }
             else
             {
@@ -836,33 +990,65 @@ VOID BiosKeyboardService(LPWORD Stack)
 
             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(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;
@@ -873,7 +1059,7 @@ VOID BiosTimeService(LPWORD Stack)
         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;
@@ -884,7 +1070,7 @@ VOID BiosTimeService(LPWORD Stack)
         default:
         {
             DPRINT1("BIOS Function INT 1Ah, AH = 0x%02X NOT IMPLEMENTED\n",
-                    HIBYTE(Eax));
+                    getAH());
         }
     }
 }
@@ -898,7 +1084,7 @@ VOID BiosSystemTimerInterrupt(LPWORD Stack)
 VOID BiosEquipmentService(LPWORD Stack)
 {
     /* Return the equipment list */
-    EmulatorSetRegister(EMULATOR_REG_AX, Bda->EquipmentList);
+    setAX(Bda->EquipmentList);
 }
 
 VOID BiosHandleIrq(BYTE IrqNumber, LPWORD Stack)
@@ -910,7 +1096,6 @@ VOID BiosHandleIrq(BYTE IrqNumber, LPWORD Stack)
         {
             /* Perform the system timer interrupt */
             EmulatorInterrupt(BIOS_SYS_TIMER_INTERRUPT);
-
             break;
         }
 
@@ -919,48 +1104,76 @@ VOID BiosHandleIrq(BYTE IrqNumber, LPWORD Stack)
         {
             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();
-            VirtualKey = MapVirtualKey(ScanCode & 0x7F, MAPVK_VSC_TO_VK);
-
-            /* Check if this is a key press or release */
-            if (!(ScanCode & (1 << 7)))
+            /* Loop while there is a scancode available */
+            do
             {
-                /* Key press */
-                if (VirtualKey == VK_NUMLOCK
-                    || VirtualKey == VK_CAPITAL
-                    || VirtualKey == VK_SCROLL)
-                {
-                    /* For toggle keys, toggle the lowest bit in the keyboard map */
-                    BiosKeyboardMap[VirtualKey] ^= ~(1 << 0);
-                }
+                /* Get the scan code and virtual key code */
+                ScanCode = KeyboardReadData();
+                VirtualKey = MapVirtualKey(ScanCode & 0x7F, MAPVK_VSC_TO_VK);
 
-                /* Set the highest bit */
-                BiosKeyboardMap[VirtualKey] |= (1 << 7);
-
-                /* Find out which character this is */
-                if (ToAscii(VirtualKey, ScanCode, BiosKeyboardMap, &Character, 0) > 0)
+                /* 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  ||
+                        VirtualKey == VK_INSERT)
+                    {
+                        /* For toggle keys, toggle the lowest bit in the keyboard map */
+                        BiosKeyboardMap[VirtualKey] ^= ~(1 << 0);
+                    }
+
+                    /* Set the highest bit */
+                    BiosKeyboardMap[VirtualKey] |= (1 << 7);
+
+                    /* Find out which character this is */
+                    Character = 0;
+                    if (ToAscii(VirtualKey, ScanCode, BiosKeyboardMap, &Character, 0) == 0)
+                    {
+                        /* Not ASCII */
+                        Character = 0;
+                    }
+
                     /* Push it onto the BIOS keyboard queue */
-                    BiosKbdBufferPush((ScanCode << 8) | (Character & 0xFF));
+                    BiosKbdBufferPush(MAKEWORD(Character, ScanCode));
+                }
+                else
+                {
+                    /* Key release, unset the highest bit */
+                    BiosKeyboardMap[VirtualKey] &= ~(1 << 7);
                 }
             }
-            else
-            {
-                /* Key release, unset the highest bit */
-                BiosKeyboardMap[VirtualKey] &= ~(1 << 7);
-            }
+            while (KeyboardReadStatus() & 1);
+
+            /* 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);
 }