svga works, optimize vga and clean up code

README.md

@@ -0,0 +1,7 @@
+# VGA logic units in 256 colors and SVGA mode usage
+
+Some code to exercise the four logic units of the VGA card in
+256 color mode, as well as code that gets SVGA mode
+working. Most of this is in C, but some of it's in assembler
+to help me learn assembler better, as well as get comfortable with
+C/asm interop on x86.

bmp.c

@@ -0,0 +1,53 @@
+#include <stdio.h>
+#include <sys/types.h>
+
+#include "bmp.h"
+#include "vesa.h"
+
+int readBMPHeader(FILE *fh, struct BMPImage *info) {
+  int sizeOfHeader;
+  char value;
+  unsigned int numberOfUsedColors;
+
+  if ((fgetc(fh) != 'B') || (fgetc(fh) != 'M')) {
+    printf("Not a BMP file\n");
+    return 1;
+  }
+
+  fseek(fh, 12, SEEK_CUR);
+
+  fread(&sizeOfHeader, sizeof(int), 1, fh);
+  fread(&info->width, sizeof(int), 1, fh);
+  fread(&info->height, sizeof(int), 1, fh);
+  fseek(fh, 2, SEEK_CUR);
+  fread(&info->bpp, sizeof(int), 1, fh);
+
+  // for gimp, you need to add colors to the color map until it hits
+  // 16 colors, then the image will be a 256 color, 8 bpp image
+  //
+  // https://www.gimp-forum.net/Thread-indexing-into-8-bit?pid=13233#pid13233
+  if (info->bpp != 8) return 1;
+
+  fseek(fh, 18, SEEK_CUR);
+  fread(&numberOfUsedColors, sizeof(unsigned int), 1, fh);
+  printf("used colors: %d\n",numberOfUsedColors);
+  if (numberOfUsedColors > 256) return 1;
+
+
+  // get down to color data
+  fseek(fh, 14 + sizeOfHeader, SEEK_SET);
+
+  fread(info->colors, sizeof(struct BMPColor), numberOfUsedColors, fh);
+
+  return 0;
+}
+
+void bmp256ColorPaletteToVESAColorPalette(struct BMPImage* bmpImage, struct VESAColor colors[]) {
+  int i;
+
+  for (i = 0; i < 256; ++i) {
+    colors[i].red = bmpImage->colors[i].red >> 2;
+    colors[i].green = bmpImage->colors[i].green >> 2;
+    colors[i].blue = bmpImage->colors[i].blue >> 2;
+  }
+}

bmp.h

@@ -0,0 +1,19 @@
+#pragma pack(push,1)
+struct BMPColor {
+  uint8_t blue;
+  uint8_t green;
+  uint8_t red;
+  uint8_t _a;
+};
+
+struct BMPImage {
+  uint16_t width;
+  uint16_t height;
+  uint16_t bpp;
+  struct BMPColor colors[256];
+  char *memoryStart;
+
+  uint16_t transparentColor;
+};
+#pragma pack(pop)
+

dpmi.c

@@ -0,0 +1,50 @@
+#include <sys/types.h>
+#include <i86.h>
+#include <dos.h>
+
+#include "dpmi.h"
+
+static uint16_t vbeDOSBufferDPMISelector = 0;
+
+/**
+* This is memory accessible to real-mode x86, so that
+* interrupts can access the data.
+*/
+void *dpmiAllocateDOSMemory(int size) {
+  union REGS regs;
+
+  // a paragraph is 16 bytes because why not
+  unsigned short sizeParagraphs = (size + 15) >> 4UL;
+
+  regs.w.ax = 0x0100;
+  regs.w.bx = sizeParagraphs;
+  int386(0x31, &regs, &regs);
+
+  vbeDOSBufferDPMISelector = regs.w.dx;
+  return (void *)(((unsigned long)regs.w.ax) << 4UL);
+}
+
+void dpmiDeallocateDOSMemory() {
+  union REGS regs;
+
+  regs.w.ax = 0x0101;
+  regs.w.dx = vbeDOSBufferDPMISelector;
+  int386(0x31, &regs, &regs);
+}
+
+/**
+ * You only need to use this if you're passing a DPMI-allocated
+ * memory struture to an interrupt via ES:DI. Otherwise, a
+ * regular int386 call is sufficient.
+ */
+void dpmiInt(int interruptNo, struct dpmiRealModeRegs *rc) {
+  union REGS regs;
+
+  // call the DPMI real mode interrupt handler
+  regs.w.ax = 0x0300;
+  regs.w.bx = interruptNo; // real mode interrupt
+  regs.w.cx = 0;           // don't mess with our stack
+  regs.x.edi = (unsigned int)rc;
+
+  int386(0x31, &regs, &regs);
+}

dpmi.h

@@ -0,0 +1,18 @@
+#ifndef __DPMI_H__
+#define __DPMI_H__
+
+#include <sys/types.h>
+
+#pragma pack(push,1)
+struct dpmiRealModeRegs {
+  uint32_t edi,esi,ebp,reserved;
+  uint32_t ebx,edx,ecx,eax;
+  uint16_t flags,es,ds,fs,gs,ip,cs,sp,ss;
+};
+#pragma paack(pop)
+
+void *dpmiAllocateDOSMemory(int size);
+void dpmiDeallocateDOSMemory();
+void dpmiInt(int interruptNo, struct dpmiRealModeRegs *);
+
+#endif

main.c

@@ -7,14 +7,14 @@ extern void __cdecl enableUnchainedVGAMode();
 extern void __cdecl enableTextMode();
 extern void __cdecl fillScreen(int);
 extern void __cdecl latchCopyCube(
-  char offset,
+  int offset,
   int writtenValue,
   int writeMode,
   int setResetRegister,
   int enableSetResetRegister
 );
 extern void __cdecl writeCube(
-  char offset,
+  int offset,
   int writeMode,
   int setResetRegister,
   int enableSetResetRegister
@@ -23,7 +23,6 @@ extern void __cdecl drawColorCube(
   char offset,
   int color
 );
-extern int __cdecl paramTest(int wow);
 
 extern void far interrupt keyboardHandler();
 extern char keyboardPressed;
@@ -40,9 +39,9 @@ void populateExampleCube(void) {
 
   outp(0x3c4, 0x02);
 
-  for (y = 0; y < 16; ++y) {
   for (x = 0; x < 16; ++x) {
     outp(0x3c5, 1 << (x % 4));
+    for (y = 0; y < 16; ++y) {
       VGA[y * PLANE_WIDTH + x / 4] = y * 16 + x;
     }
   }
@@ -50,20 +49,20 @@ void populateExampleCube(void) {
 
 int main(void) {
   // activate unchained vga mode
-  // set up 8 very clear colors
   // place some data into video memory to be latch copied
   // show off the following
-  // * latch copies
+  // * latch copies with logical operators
-  // * masking
+  // * plane selection
   // * barrel shifting
-
+  // * set/reset
-  int i,j;
 
   int9Save = _dos_getvect(9);
   _dos_setvect(9, keyboardHandler);
 
   enableUnchainedVGAMode();
 
+  // set a very bright color as the last possible color to make
+  // set/reset usage more visible
   outp(0x3c8, 255);
   outp(0x3c9, 63);
   outp(0x3c9, 63);
@@ -71,27 +70,28 @@ int main(void) {
 
   fillScreen(0);
 
-  /*
   populateExampleCube();
-  latchCopyCube(8, 0, 0x00, 0, 0);
-  latchCopyCube(12, 127, 0x00, 0, 0);
-  latchCopyCube(16, 0, 0x08, 0, 0);
-  latchCopyCube(20, 127, 0x08, 0, 0);
-  latchCopyCube(24, 0, 0x10, 0, 0);
-  latchCopyCube(28, 127, 0x10, 0, 0);
-  latchCopyCube(32, 0, 0x18, 0, 0);
-  latchCopyCube(36, 127, 0x18, 0, 0);
-  latchCopyCube(40, 0, 0x18 + 0x04, 0, 0);
-  latchCopyCube(44, 127, 0x18 + 0x04, 0, 0);
-  latchCopyCube(64, 0, 0x10, 7, 7);
-  latchCopyCube(68, 0, 0x10, 7, 0);
-  latchCopyCube(72, 0, 0x10, 0, 7);
-  */
 
-  writeCube(0, 0x10, 0, 0);
+  latchCopyCube(80 * 20, 0, 0x00, 0, 0);
-  writeCube(4, 0x10, 7, 0);
+  latchCopyCube(80 * 20 + 5, 127, 0x00, 0, 0);
-  writeCube(8, 0x10, 7, 7);
+  latchCopyCube(80 * 20 + 10, 0, 0x08, 0, 0);
-  writeCube(12, 0x10, 0, 7);
+  latchCopyCube(80 * 20 + 15, 127, 0x08, 0, 0);
+  latchCopyCube(80 * 20 + 20, 0, 0x10, 0, 0);
+  latchCopyCube(80 * 20 + 25, 127, 0x10, 0, 0);
+  latchCopyCube(80 * 20 + 30, 0, 0x18, 0, 0);
+  latchCopyCube(80 * 20 + 35, 127, 0x18, 0, 0);
+  latchCopyCube(80 * 20 + 40, 127, 0x10, 1, 3);
+
+  latchCopyCube(80 * 40, 0, 0x18 + 0x04, 0, 0);
+  latchCopyCube(80 * 40 + 5, 127, 0x18 + 0x04, 0, 0);
+
+  latchCopyCube(80 * 40 + 10, 0, 0x10, 3, 3);
+  latchCopyCube(80 * 40 + 15, 0, 0x10, 1, 3);
+  latchCopyCube(80 * 40 + 20, 0, 0x10, 0, 3);
+
+  writeCube(5, 0x00, 1, 3);
+  writeCube(10, 0x00, 0, 3);
+  writeCube(15, 0x00, 3, 0);
 
   while (keyboardPressed == 0);
 

makefile

@@ -1,4 +1,5 @@
 obj = vga.o main.o
+svga_obj = dpmi.o bmp.o vesa.o svga.o
 
 .c.o:
 	wcc386 -q -bt=dos $<
@@ -8,3 +9,8 @@ obj = vga.o main.o
 
 main.exe: $(obj)
 	wcl386 -fe=main -bt=dos -l=dos4g $(obj)
+
+svga.exe: $(svga_obj)
+	wcl386 -fe=svga -bt=dos -l=dos4g $(svga_obj)
+
+all: main.exe svga.exe

svga.c

@@ -0,0 +1,102 @@
+#include <stddef.h>
+#include <stdio.h>
+#include <conio.h>
+#include <dos.h>
+#include <i86.h>
+#include <memory.h>
+#include <sys/types.h>
+
+/*
+
+Set up a 640x480x256 Super VGA screen and load a bitmap image into it.
+Wait 10 seconds, then exit the program.
+
+Since this is going through DOS/4GW, we need to use the DOS Protected
+Mode Interface (DPMI) to work with x86 Real Mode RAM so the VESA
+interrupt functions have a place to accept and dump data.
+
+There were a lot of scattered examples as to how to get SVGA going, but:
+
+* most were written in assembler and, while I can read assembler, I don't
+  prefer to, especially in the early learning stages of a bunch of new concepts
+* very few of them covered the whole pipeline of query display, then
+  set it up, then do something real with it
+
+So here's this one. Enjoy.
+
+John
+
+*/
+
+// built from a combination of:
+//
+// * http://www.pennelynn.com/Documents/CUJ/HTML/14.08/KREHBIEL/KREHL1.HTM
+// * https://forum.osdev.org/viewtopic.php?f=2&t=30186
+// * https://github.com/joncampbell123/doslib/blob/master/hw/vesa/vesa.c
+// * https://stackoverflow.com/questions/3318410/pragma-pack-effect
+// * https://www.delorie.com/djgpp/doc/dpmi/ch4.4.html
+// * https://delorie.com/djgpp/doc/dpmi/api/310100.html
+// * http://www.monstersoft.com/tutorial1/VESA_intro.html
+// * http://www.delorie.com/djgpp/doc/ug/graphics/vesa.html.en
+// * https://lhlaurini.xyz/VESA_Video_Modes
+// * https://stackoverflow.com/questions/58328264/retrieve-list-of-vesa-video-modes-from-int-10h-ax-4f00h
+// * https://github.com/execomrt/vesa2/blob/main/vbelib/vbecore.c#L262
+// * https://pdos.csail.mit.edu/6.828/2004/readings/hardware/vbe3.pdf
+
+#include "vesa.h"
+#include "bmp.h"
+#include "dpmi.h"
+
+/**
+
+If you want to enumerate all the possible modes to inspect, you would need
+something like this to turn the segment:offset data into array elements you can
+address in 32-bit DOS.
+
+uint16_t *modePtr;
+
+modePtr = (uint16_t*)((vbeInfo.VideoModeSegment << 4) + vbeInfo.VideoModeOffset);
+for (i = 0; i < 4; ++i) {
+  printf("Available mode %d: 0x%x\n", i, modePtr[i]);
+}
+
+*/
+
+int main(void) {
+  int x, y;
+  FILE *bmp;
+  struct BMPImage bmpImage;
+  struct VESAColor colors[256];
+  struct VbeInfo vbeInfo;
+  struct VbeModeInfo vbeModeInfo;
+
+  setupVESAHandling();
+
+  vbeGetVBEInfo(&vbeInfo);
+  printf("VBE Version: 0x%x\n", vbeInfo.VbeVersion);
+
+  bmp = fopen("chkn640.bmp", "rb");
+  readBMPHeader(bmp, &bmpImage);
+  bmp256ColorPaletteToVESAColorPalette(&bmpImage, colors);
+
+  // we could enumerate all the modes, but in DOSBox-X's default S3 VGA card,
+  // 0x101 is 640x480x256, and that's where I'm running this. it's more correct
+  // to select the mode based on examining capabilities for all available
+  // modes, but...eh.
+  startSVGAMode(0x101, &vbeModeInfo);
+  setVESAColorPalette(colors);
+
+  for (y = 0; y < vbeModeInfo.YResolution; ++y) {
+    for (x = 0; x < vbeModeInfo.XResolution; ++x) {
+      setPixel(x, vbeModeInfo.YResolution - y, fgetc(bmp));
+    }
+  }
+
+  fclose(bmp);
+
+  delay(10000);
+
+  teardownVESAHandling();
+
+  return 0;
+}

vesa.c

@@ -0,0 +1,133 @@
+#include <memory.h>
+#include <i86.h>
+#include <dos.h>
+
+#include "dpmi.h"
+#include "vesa.h"
+
+char far *VGA = (char*)0xA0000;
+long windowSizeBytes, bytesPerScanLine;
+void *dosmem;
+int currentWindowPosition;
+
+#define MAX_COLORS (256)
+
+void setupVESAHandling() {
+  dosmem = dpmiAllocateDOSMemory(1024);
+}
+
+void teardownVESAHandling() {
+  dpmiDeallocateDOSMemory();
+}
+
+void addDOSMemToRealModeRegs(struct dpmiRealModeRegs *rc) {
+  rc->edi = (unsigned long)dosmem & 0xf;
+  rc->es = (unsigned long)dosmem >> 4;
+}
+
+void vbeGetModeInfo(
+  uint16_t mode,
+  struct VbeModeInfo *modeInfo
+) {
+  struct dpmiRealModeRegs rc={0};
+
+  _fmemset(dosmem, 0, sizeof(struct VbeModeInfo));
+
+  rc.eax = 0x4f01;
+  rc.ecx = mode;
+  addDOSMemToRealModeRegs(&rc);
+
+  dpmiInt(0x10, &rc);
+
+  _fmemcpy(modeInfo, dosmem, sizeof(struct VbeModeInfo));
+}
+
+int vbeGetVBEInfo(
+  struct VbeInfo far *p
+) {
+  struct dpmiRealModeRegs rc={0};
+
+  // request VBE2 extended info
+  _fmemset(p, 0, sizeof(struct VbeInfo));
+  _fmemcpy(p->VbeSignature,"VBE2",4);
+  _fmemcpy(dosmem, p, sizeof(struct VbeInfo));
+
+  rc.eax = 0x4f00;
+  addDOSMemToRealModeRegs(&rc);
+
+  dpmiInt(0x10, &rc);
+
+  _fmemcpy(p, dosmem, sizeof(struct VbeInfo));
+
+  return 0;
+}
+
+void startSVGAMode(int mode, struct VbeModeInfo *vbeModeInfo) {
+  /**
+   * Side effects: sets some internal variables for window and scanline sizes
+   */
+  union REGS regs;
+
+  vbeGetModeInfo(mode, vbeModeInfo);
+
+  windowSizeBytes = 1024L * vbeModeInfo->WinSize;
+  bytesPerScanLine = vbeModeInfo->BytesPerScanLine;
+
+  regs.w.ax = 0x4f02;
+  regs.w.bx = mode;
+  int386(0x10, &regs, &regs);
+
+  vbeSetWindow(0, 0);
+}
+
+void vbeSetWindow(int window, int position) {
+  /**
+   * Side effects: tracks current window position
+   */
+  union REGS regs;
+
+  regs.w.ax = 0x4f05;
+  regs.h.bh = 0;
+  regs.h.bl = window;
+  regs.w.dx = position;
+  int386(0x10, &regs, &regs);
+
+  currentWindowPosition = position;
+}
+
+void maybeVbeSetWindow(int window, int position) {
+  if (currentWindowPosition == position) return;
+
+  vbeSetWindow(window, position);
+}
+
+void setVESAColorPalette(
+  struct VESAColor *colors
+) {
+  struct dpmiRealModeRegs rc={0};
+  int i;
+
+  _fmemcpy(dosmem, colors, MAX_COLORS * 4);
+
+  // prep our real mode interrupt call
+  rc.eax = 0x4f09;
+  rc.ebx = 0;
+  rc.ecx = MAX_COLORS;
+  rc.edx = 0;
+  addDOSMemToRealModeRegs(&rc);
+
+  dpmiInt(0x10, &rc);
+}
+
+void setPixel(int x, int y, int color) {
+  long absolutePosition = (long)y * bytesPerScanLine + x;
+  long windowPosition = absolutePosition / windowSizeBytes;
+  long windowOffset = absolutePosition % windowSizeBytes;
+
+  /**
+  * This will not shift window positions unless needed.
+  */
+  maybeVbeSetWindow(0, windowPosition);
+
+  VGA[windowOffset] = color;
+}

vesa.h

@@ -0,0 +1,57 @@
+#ifndef __VESA_H__
+#define __VESA_H__
+
+#include <sys/types.h>
+
+#pragma pack(push,1)
+struct VbeInfo {
+  char VbeSignature[4];
+  uint16_t VbeVersion;
+  uint32_t OemStringPtr;
+  uint32_t Capabilities;
+
+  uint16_t VideoModeOffset;
+  uint16_t VideoModeSegment;
+
+  uint16_t TotalMemory;
+
+  int OemSoftwareRev;
+  char far *OemVendorNamePtr;
+  char far *OemProductNamePtr;
+  char far *OenProductRevPtr;
+  char Reserved[222];
+  char OemData[256];
+};
+
+struct VbeModeInfo {
+  uint16_t ModeAttributes;
+  uint8_t WinAAttributes;
+  uint8_t WinBAttributes;
+  uint16_t WinGranularity;
+  uint16_t WinSize;
+  uint16_t WinASegment;
+  uint16_t WinBSegment;
+  void *WindowFunction;
+  uint16_t BytesPerScanLine;
+  uint16_t XResolution;
+  uint16_t YResolution;
+  uint8_t XCharSize;
+  uint8_t YCharSize;
+  uint8_t NumberOfPlanes;
+  uint8_t BitsPerPixel;
+  uint8_t NumberOfBanks;
+  uint8_t MemoryModel;
+  uint8_t BankSize;
+  uint8_t NumberOfImagePages;
+  char more[500];
+};
+
+struct VESAColor {
+  uint8_t blue;
+  uint8_t green;
+  uint8_t red;
+  uint8_t _a;
+};
+#pragma pack(pop)
+
+#endif

vga.asm

@@ -4,7 +4,6 @@ PUBLIC _fillScreen
 PUBLIC _latchCopyCube
 PUBLIC _drawColorCube
 PUBLIC _writeCube
-PUBLIC _paramTest
 PUBLIC _keyboardPressed
 PUBLIC keyboardHandler_
 
@@ -39,6 +38,7 @@ keyboardHandler_:
   push eax
   push ebx
 
+  ; put scancode into bl
   in al, 0x60
   mov bl, al
 
@@ -50,6 +50,7 @@ keyboardHandler_:
   xchg ah, al
   out 0x61, al
 
+  ; a key is pressed if the topmost bit of the scancode is 0
   and bl,0x80
   jne _keyboardHandler_notKeydown
 
@@ -62,8 +63,10 @@ _keyboardHandler_notKeydown:
   pop ebx
   pop eax
   sti
+  ; return from the interrupt in a way that preserves the calling stack
   iretd
 
+
 _enableUnchainedVGAMode:
   mov ax,0x0013;
   int 0x10
@@ -85,12 +88,14 @@ _enableUnchainedVGAMode:
 
   ret
 
+
 _enableTextMode:
   mov ah,0h
   mov al,DISPLAY_MODE_TEXT
   int 10h
   ret
 
+
 _fillScreen:
   push ebp
   mov ebp, esp
@@ -98,34 +103,45 @@ _fillScreen:
   ; set all bitplanes
   mov dx, VGA_SEQUENCE_CONTROLLER_INDEX
   mov al, 0x02
-  mov ah, 0fh
+  mov ah, 0x0f
   out dx, ax
 
+  push ebx
+    push ecx
+      push edx
+        sub eax,eax
         mov al, [ebp+8] ; requested color
 
-  mov cx, 16000 ; loop index
+        ; we can write a long at a time and let the bus
-  mov ebx, 0xa0000
+        ; and BIU work out the details. on a 16-bit ISA
+        ; bus with a 16-bit VGA card, this will be faster
+        ; than byte writes
+        sub edx,edx
+        add dl,al
+        shl edx,8
+        add dl,al
+        shl edx,8
+        add dl,al
+        shl edx,8
+        add dl,al
+
+        ; with all planes selected we only need to fill 16000 bytes of data
+        ; and we're filling them four bytes at a time
+        mov cx, 4000
+        mov ebx, 0xA0000 ; VGA base address
 _fillScreen_loop:
-  mov BYTE PTR [ebx], al
+        mov DWORD PTR [ebx], edx
-  inc ebx
+        add ebx,4
         loop _fillScreen_loop
+      pop edx
+    pop ecx
+  pop ebx
 
   mov esp,ebp
   pop ebp
 
   ret
 
-_paramTest:
-  push ebp
-  mov ebp,esp
-
-  mov eax,0
-  mov al, BYTE PTR [ebp+8]
-
-  mov esp,ebp
-  pop ebp
-  ret
-
 
 _drawColorCube:
   push ebp
@@ -165,6 +181,7 @@ _drawColorCube:
 _drawColorCube_yLoop:
   mov [ebp-4],cx
 
+  ; put the VGA write offset into eax
   mov eax,[ebp-4]
   dec eax
   mov bl,80
@@ -174,7 +191,7 @@ _drawColorCube_yLoop:
 
   mov cx, 4
 _drawColorCube_xLoop:
-  mov [eax],bh ; write color
+  mov [eax],bh ; write color four bytes at a time
   inc eax
 
   loop _drawColorCube_xLoop
@@ -190,15 +207,15 @@ _drawColorCube_xLoop:
   ret
 
 
-
-
 _writeCube:
   push ebp
   mov ebp,esp
   ; we need a y variable
   sub esp,4
-  ; and a current color
+
-  sub esp,4
+  push ebx
+  push ecx
+  push edx
 
   ; [ebp+8] is x offset
   ; [ebp+12] is data rotate
@@ -222,57 +239,66 @@ _writeCube:
   mov ah, [ebp+20]
   out dx, ax
 
-  mov DWORD PTR [ebp-8],0
   mov DWORD PTR [ebp-4],16
   mov cx,[ebp-4]
 
 _writeCube_yLoop:
   mov [ebp-4],cx
 
+  ; determine the offset within VGA memory to write this line to
+  ; and put it in eax
   mov eax,[ebp-4]
   dec eax
   mov bl,80
   mul bl
-  add eax,0xa0000
+  add eax,0xA0000
 
+  ; 16 pixels across
   mov cx,16
 _writeCube_xLoop:
-  ; get the current plane
+  push eax
+    ; get the plane to write to in bl
     push ecx
-    dec cl
+      dec ecx
-    and cl,0x03
+      and ecx,0x03
-
-    jne _writeCube_dontAdvanceVGA
-    inc eax
-_writeCube_dontAdvanceVGA:
-
       mov bl,1
-    sal bl,cl
+      shl bl,cl
     pop ecx
 
-  push eax
     ; write to one plane
     mov dx, VGA_SEQUENCE_CONTROLLER_INDEX
     mov al, VGA_SEQUENCE_CONTROLLER_MAP_MASK_MODE
     mov ah, bl
     out dx, ax
+
+    ; select color to write into bl
+    mov ax,[ebp-4]
+    dec ax
+    mov bl,16
+    mul bl
+    add al,cl
+    mov bl,al
   pop eax
 
-  ;mov ebx,[ebp+8]
+  push edx
-
     push ecx
-    mov ecx,[ebp+8]
+      ; select the correct pixel offset (x / 4) and write the color
-    mov bl,[ebp-8]
+      dec ecx
+      shr ecx,2
+      add ecx,[ebp+8]
       mov BYTE PTR [eax+ecx],bl
-    inc bl
-    mov [ebp-8],bl
     pop ecx
+  pop edx
 
   loop _writeCube_xLoop
 
   mov cx,[ebp-4]
   loop _writeCube_yLoop
 
+  pop edx
+  pop ecx
+  pop ebx
+
   mov esp,ebp
   pop ebp
   ret
@@ -359,6 +385,8 @@ _latchCopyCube_xLoop:
   ; loop until y is 0
   loop _latchCopyCube_yLoop
 
+_done:
+
   ; put things back
   pop ebx
   pop edx