X-Git-Url: http://git.jankratochvil.net/?p=reactos.git;a=blobdiff_plain;f=drivers%2Fdd%2Fvga%2Fdisplay%2Fvgavideo%2Fvgavideo.c;fp=drivers%2Fdd%2Fvga%2Fdisplay%2Fvgavideo%2Fvgavideo.c;h=89696f9519b34468ae04db16e0dd484556c69f35;hp=7d9aa8d025aa153214708a50c3d87680f895aca4;hb=7c0cf90e3b750f1f0dc83b2eec9e5c68a512c30f;hpb=ee8b63255465d8c28be3e7bd11628015708fc1ab

diff --git a/drivers/dd/vga/display/vgavideo/vgavideo.c b/drivers/dd/vga/display/vgavideo/vgavideo.c
index 7d9aa8d..89696f9 100644
--- a/drivers/dd/vga/display/vgavideo/vgavideo.c
+++ b/drivers/dd/vga/display/vgavideo/vgavideo.c
@@ -12,6 +12,7 @@ int bit8[640];
 int startmasks[8];
 int endmasks[8];
 char* vidmem;
+static ULONG UnpackPixel[256];
 
 static unsigned char saved_SEQ_mask;	/* 0x02 */
 static unsigned char saved_GC_eSR;	/* 0x01 */
@@ -23,6 +24,9 @@ static unsigned char leftMask;
 static int byteCounter;
 static unsigned char rightMask;
 
+#define READ_REGISTER_UCHAR(p) (*((PUCHAR)(p)))
+#define WRITE_REGISTER_UCHAR(p,c) (*((PCHAR)(p))) = (c)
+
 INT abs(INT nm)
 {
   if(nm<0)
@@ -54,11 +58,11 @@ div_t div(int num, int denom)
   return r;
 }
 
-int mod(int num, int denom)
+/*int mod(int num, int denom)
 {
   div_t dvt = div(num, denom);
   return dvt.rem;
-}
+}*/
 
 BYTE bytesPerPixel(ULONG Format)
 {
@@ -66,32 +70,31 @@ BYTE bytesPerPixel(ULONG Format)
   // FIXME: GDI bitmaps are supposed to be pixel-packed. Right now if the
   // pixel size if < 1 byte we expand it to 1 byte for simplicities sake
 
-  if(Format==BMF_1BPP)
+  switch ( Format )
   {
+  case BMF_1BPP:
     return 1;
-  } else
-  if((Format==BMF_4BPP) || (Format==BMF_4RLE))
-  {
+
+  case BMF_4BPP:
+  case BMF_4RLE:
     return 1;
-  } else
-  if((Format==BMF_8BPP) || (Format==BMF_8RLE))
-  {
+
+  case BMF_8BPP:
+  case BMF_8RLE:
     return 1;
-  } else
-  if(Format==BMF_16BPP)
-  {
+
+  case BMF_16BPP:
     return 2;
-  } else
-  if(Format==BMF_24BPP)
-  {
+
+  case BMF_24BPP:
     return 3;
-  } else
-  if(Format==BMF_32BPP)
-  {
+  
+  case BMF_32BPP:
     return 4;
-  }
 
-  return 0;
+  default:
+    return 0;
+  }
 }
 
 VOID vgaPreCalc()
@@ -106,17 +109,15 @@ VOID vgaPreCalc()
   startmasks[5] = 31;
   startmasks[6] = 63;
   startmasks[7] = 127;
-  startmasks[8] = 255;
-
-  endmasks[0] = 128;
-  endmasks[1] = 192;
-  endmasks[2] = 224;
-  endmasks[3] = 240;
-  endmasks[4] = 248;
-  endmasks[5] = 252;
-  endmasks[6] = 254;
-  endmasks[7] = 255;
-  endmasks[8] = 255;
+
+  endmasks[0] = 0;
+  endmasks[1] = 128;
+  endmasks[2] = 192;
+  endmasks[3] = 224;
+  endmasks[4] = 240;
+  endmasks[5] = 248;
+  endmasks[6] = 252;
+  endmasks[7] = 254;
 
   for(j=0; j<80; j++)
   {
@@ -159,6 +160,19 @@ VOID vgaPreCalc()
 	(((j >> 6) & 0x1) << 1) |
 	(((j >> 7) & 0x1) << 0);
     }
+
+  for (j = 0; j < 256; j++)
+    {
+      UnpackPixel[j] =
+	(((j >> 0) & 0x1) << 4) |
+	(((j >> 1) & 0x1) << 0) |
+	(((j >> 2) & 0x1) << 12) |
+	(((j >> 3) & 0x1) << 8) |
+	(((j >> 4) & 0x1) << 20) |
+	(((j >> 5) & 0x1) << 16) |
+	(((j >> 6) & 0x1) << 28) |
+	(((j >> 7) & 0x1) << 24);
+    }
 }
 
 void
@@ -196,8 +210,8 @@ VOID vgaPutPixel(INT x, INT y, UCHAR c)
 
   offset = xconv[x]+y80[y];
 
-  WRITE_PORT_UCHAR((PUCHAR)0x3ce,0x08);
-  WRITE_PORT_UCHAR((PUCHAR)0x3cf,maskbit[x]);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D,maskbit[x]);
 
   a = READ_REGISTER_UCHAR(vidmem + offset);
   WRITE_REGISTER_UCHAR(vidmem + offset, c);
@@ -210,8 +224,8 @@ VOID vgaPutByte(INT x, INT y, UCHAR c)
   offset = xconv[x]+y80[y];
 
   // Set the write mode
-  WRITE_PORT_UCHAR((PUCHAR)0x3ce,0x08);
-  WRITE_PORT_UCHAR((PUCHAR)0x3cf,0xff);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D,0xff);
 
   WRITE_REGISTER_UCHAR(vidmem + offset, c);
 }
@@ -220,13 +234,13 @@ VOID vgaGetByte(ULONG offset,
                 UCHAR *b, UCHAR *g,
                 UCHAR *r, UCHAR *i)
 {
-  WRITE_PORT_USHORT((PUSHORT)0x03ce, 0x0304);
+  WRITE_PORT_USHORT((PUSHORT)GRA_I, 0x0304);
   *i = READ_REGISTER_UCHAR(vidmem + offset);
-  WRITE_PORT_USHORT((PUSHORT)0x03ce, 0x0204);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
   *r = READ_REGISTER_UCHAR(vidmem + offset);
-  WRITE_PORT_USHORT((PUSHORT)0x03ce, 0x0104);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x01);
   *g = READ_REGISTER_UCHAR(vidmem + offset);
-  WRITE_PORT_USHORT((PUSHORT)0x03ce, 0x0004);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
   *b = READ_REGISTER_UCHAR(vidmem + offset);
 }
 
@@ -260,71 +274,72 @@ BOOL vgaHLine(INT x, INT y, INT len, UCHAR c)
   ULONG orgpre1, orgx, midpre1;
   ULONG ileftpix, imidpix, irightpix;
 
-  orgx=x;
+  orgx = x;
 
-  if(len<8)
+  /*if ( len < 8 )
   {
-    for (i=x; i<x+len; i++)
-      vgaPutPixel(i, y, c);
+    for (i = x; i < x+len; i++ )
+      vgaPutPixel ( i, y, c );
 
-   return TRUE;
-  }
+    return TRUE;
+  }*/
 
   // Calculate the left mask pixels, middle bytes and right mask pixel
-  ileftpix = 8-mod(x, 8);
-  irightpix = mod(x+len, 8);
+  ileftpix = 7 - mod8(x-1);
+  irightpix = mod8(x+len);
   imidpix = (len-ileftpix-irightpix) / 8;
 
-  if(ileftpix == 8)
+  pre1 = xconv[(x-1)&~7] + y80[y];
+  orgpre1=pre1;
+
+  // check for overlap ( very short line )
+  if ( (ileftpix+irightpix) > len )
   {
-    ileftpix = 0;
-    imidpix++;
-  }
+    int mask = startmasks[ileftpix] & endmasks[irightpix];
+    // Write left pixels
+    WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);     // set the mask
+    WRITE_PORT_UCHAR((PUCHAR)GRA_D,mask);
 
-  pre1=xconv[x-(8-ileftpix)]+y80[y];
-  orgpre1=pre1;
+    a = READ_REGISTER_UCHAR(vidmem + pre1);
+    WRITE_REGISTER_UCHAR(vidmem + pre1, c);
+
+    return TRUE;
+  }
 
   // Left
-  if(ileftpix>0)
+  if ( ileftpix > 0 )
   {
     // Write left pixels
-    WRITE_PORT_UCHAR((PUCHAR)0x3ce,0x08);     // set the mask
-    WRITE_PORT_UCHAR((PUCHAR)0x3cf,startmasks[ileftpix]);
+    WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);     // set the mask
+    WRITE_PORT_UCHAR((PUCHAR)GRA_D,startmasks[ileftpix]);
 
     a = READ_REGISTER_UCHAR(vidmem + pre1);
     WRITE_REGISTER_UCHAR(vidmem + pre1, c);
 
     // Prepare new x for the middle
-    x=orgx+8;
+    x = orgx + 8;
   }
 
-  if(imidpix>0)
+  if ( imidpix > 0 )
   {
-    midpre1=xconv[x]+y80[y];
+    midpre1 = xconv[x] + y80[y];
 
     // Set mask to all pixels in byte
-    WRITE_PORT_UCHAR((PUCHAR)0x3ce, 0x08);
-    WRITE_PORT_UCHAR((PUCHAR)0x3cf, 0xff);
+    WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);
+    WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0xff);
     memset(vidmem+midpre1, c, imidpix); // write middle pixels, no need to read in latch because of the width
   }
 
-  if(irightpix>0)
+  if ( irightpix > 0 )
   {
-    x=orgx+len-irightpix;
-
-    for(i=x; i<x+irightpix; i++)
-    {
-      vgaPutPixel(i, y, c);
-    }
-
-/*  pre1=xconv[x]+y80[y];
+    x = orgx + len - irightpix;
+    pre1 = xconv[x] + y80[y];
 
     // Write right pixels
-    WRITE_PORT_UCHAR((PUCHAR)0x3ce,0x08);     // set the mask bits
-    WRITE_PORT_UCHAR((PUCHAR)0x3cf, endmasks[irightpix]);
-
+    WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);     // set the mask bits
+    WRITE_PORT_UCHAR((PUCHAR)GRA_D, endmasks[irightpix]);
     a = READ_REGISTER_UCHAR(vidmem + pre1);
-    WRITE_REGISTER_UCHAR(vidmem + pre1, c); */
+    WRITE_REGISTER_UCHAR(vidmem + pre1, c);
   }
 
   return TRUE;
@@ -337,8 +352,12 @@ BOOL vgaVLine(INT x, INT y, INT len, UCHAR c)
 
   offset = xconv[x]+y80[y];
 
-  WRITE_PORT_UCHAR((PUCHAR)0x3ce,0x08);       // set the mask
-  WRITE_PORT_UCHAR((PUCHAR)0x3cf,maskbit[x]);
+#ifdef VGA_PERF
+  vgaSetBitMaskRegister ( maskbit[x] );
+#else
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);       // set the mask
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D,maskbit[x]);
+#endif
 
   for(i=y; i<y+len; i++)
   {
@@ -373,7 +392,101 @@ BOOL VGADDIIntersectRect(PRECTL prcDst, PRECTL prcSrc1, PRECTL prcSrc2)
 }
 
 void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
+{
+  ULONG plane;
+  ULONG left = x >> 3;
+  ULONG shift = x - (x & ~0x7);  
+  UCHAR pixel, nextpixel;    
+  ULONG rightcount;
+  ULONG i, j;
+  ULONG stride = w >> 3;
+  
+  /* Calculate the number of rightmost bytes not in a dword block. */
+  if (w >= 8)
+    {
+      rightcount = w % 8;
+    }
+  else
+    {
+      stride = 0;
+      rightcount = w;
+    }
+
+  /* Reset the destination. */
+  memset(b, 0, h * Dest_lDelta);
+
+  for (plane = 0; plane < 4; plane++)
+    {
+      PUCHAR dest = b;
+      
+      /* Select the plane we are reading in this iteration. */
+      WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x04);
+      WRITE_PORT_UCHAR((PUCHAR)GRA_D, plane);
+      
+      for (j = 0; j < h; j++)
+	{
+	  PULONG destline = (PULONG)dest;
+	  PUCHAR src = vidmem + (y + j) * SCREEN_STRIDE + left;
+	  /* Read the data for one plane for an eight aligned pixel block. */
+	  nextpixel = PreCalcReverseByte[READ_REGISTER_UCHAR(src)];
+	  for (i = 0; i < stride; i++, src++, destline++)
+	    {
+	      /* Form the data for one plane for an aligned block in the destination. */
+	      pixel = nextpixel;
+	      pixel >>= shift;
+	      
+	      nextpixel = PreCalcReverseByte[READ_REGISTER_UCHAR(src + 1)];
+	      pixel |= (nextpixel << (8 - shift));
+
+	      /* Expand the plane data to 'chunky' format and store. */
+	      *destline |= (UnpackPixel[pixel] << plane);
+	    }
+	  /* Handle any pixels not falling into a full block. */
+	  if (rightcount != 0)
+	    {
+	      ULONG row;
+
+	      /* Form the data for a complete block. */
+	      pixel = nextpixel;
+	      pixel >>= shift;
+	      
+	      nextpixel = PreCalcReverseByte[READ_REGISTER_UCHAR(src + 1)];
+	      pixel |= (nextpixel << (8 - shift));
+	      
+	      row = UnpackPixel[pixel] << plane;
+
+	      /* Store the data for each pixel in the destination. */
+	      for (i = 0; i < rightcount; i++)
+		{
+		  ((PUCHAR)destline)[i] |= (row & 0xFF);
+		  row >>= 8;
+		}
+	    }
+	  dest += Dest_lDelta;
+	}
+    }
 
+#ifdef VGA_VERIFY
+  for (j = 0; j < h; j++)
+    {
+      for (i = 0; i < w; i+=2)
+	{
+	  UCHAR c1, c2;
+	  ULONG mask = (i < (w - 1)) ? 0xFF : 0xF0;
+
+	  c1 = (vgaGetPixel(x + i, y + j) << 4) | (vgaGetPixel(x + i + 1, y + j));
+	  c2 = ((PUCHAR)b)[(j * Dest_lDelta) + (i >> 1)];
+	  if ((c1 & mask) != (c2 & mask))
+	    {
+	      __asm__("int $3\n\t" : /* no outputs */ : /* no inputs */);
+	    }
+	}
+    }
+#endif /* VGA_VERIFY */
+}
+
+#if 0
+void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
 //  DIB blt from the VGA.
 //  For now we just do slow reads -- pixel by pixel, packing each one into the correct 4BPP format.
 {
@@ -385,7 +498,7 @@ void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
   BYTE  b1, b2;
 
   // Check if the width is odd
-  if(mod(w, 2)>0)
+  if(mod2(w)>0)
   {
     edgePixel = TRUE;
     x2 -= 1;
@@ -404,7 +517,7 @@ void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
     if(edgePixel == TRUE)
     {
       b1 = vgaGetPixel(x2, j);
-      *pb = b1;
+      *pb = b1 << 4;
       pb++;
     }
 
@@ -412,49 +525,98 @@ void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
     pb = opb; // new test code
   }
 }
+#endif
 
+/* DIB blt to the VGA. */
 void DIB_BltToVGA(int x, int y, int w, int h, void *b, int Source_lDelta)
-
-//  DIB blt to the VGA.
-//  For now we just do slow writes -- pixel by pixel, packing each one into the correct 4BPP format.
 {
-  PBYTE  pb = b, opb = b;
-  BOOLEAN  edgePixel = FALSE;
-  ULONG  i, j;
-  ULONG  x2 = x + w;
-  ULONG  y2 = y + h;
-  BYTE  b1, b2;
-
-  // Check if the width is odd
-  if(mod(w, 2)>0)
-  {
-    edgePixel = TRUE;
-    x2 -= 1;
-  }
+  PBYTE pb, opb = b;
+  ULONG i, j;
+  ULONG x2 = x + w;
+  ULONG y2 = y + h;
+  ULONG offset;
+  UCHAR a;
 
-  for (j=y; j<y2; j++)
-  {
-    for (i=x; i<x2; i+=2)
+  for (i = x; i < x2; i++)
     {
-      b1 = (*pb & 0xf0) >> 4;
-      b2 = *pb & 0x0f;
-      vgaPutPixel(i,   j, b1);
-      vgaPutPixel(i+1, j, b2);
-      pb++;
+      pb = opb;
+      offset = xconv[i] + y80[y];
+
+      WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);       // set the mask
+      WRITE_PORT_UCHAR((PUCHAR)GRA_D, maskbit[i]);
+
+      if (0 == ((i - x) % 2))
+	{
+	  for (j = y; j < y2; j++)
+	    {
+	      a = READ_REGISTER_UCHAR(vidmem + offset);
+	      WRITE_REGISTER_UCHAR(vidmem + offset, (*pb & 0xf0) >> 4);
+	      offset += 80;
+	      pb += Source_lDelta;
+	    }
+	}
+      else
+	{
+	  for (j = y; j < y2; j++)
+	    {
+	      a = READ_REGISTER_UCHAR(vidmem + offset);
+	      WRITE_REGISTER_UCHAR(vidmem + offset, *pb & 0x0f);
+	      offset += 80;
+	      pb += Source_lDelta;
+	    }
+	}
+
+      if (0 != ((i - x) % 2))
+	{
+	  opb++;
+	}
     }
+}
 
-    if(edgePixel == TRUE)
+/* DIB blt to the VGA. */
+void DIB_BltToVGAWithXlate(int x, int y, int w, int h, void *b, int Source_lDelta, PXLATEOBJ Xlate)
+{
+  PBYTE pb, opb = b;
+  ULONG i, j;
+  ULONG x2 = x + w;
+  ULONG y2 = y + h;
+  ULONG offset;
+  UCHAR a;
+
+  for (i = x; i < x2; i++)
     {
-      b1 = *pb;
-      vgaPutPixel(x2, j, b1);
-      pb++;
+      pb = opb;
+      offset = xconv[i] + y80[y];
+
+      WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);       // set the mask
+      WRITE_PORT_UCHAR((PUCHAR)GRA_D, maskbit[i]);
+
+      if (0 == ((i - x) % 2))
+	{
+	  for (j = y; j < y2; j++)
+	    {
+	      a = READ_REGISTER_UCHAR(vidmem + offset);
+	      WRITE_REGISTER_UCHAR(vidmem + offset, XLATEOBJ_iXlate(Xlate, (*pb & 0xf0) >> 4));
+	      offset += 80;
+	      pb += Source_lDelta;
+	    }
+	}
+      else
+	{
+	  for (j = y; j < y2; j++)
+	    {
+	      a = READ_REGISTER_UCHAR(vidmem + offset);
+	      WRITE_REGISTER_UCHAR(vidmem + offset, XLATEOBJ_iXlate(Xlate, *pb & 0x0f));
+	      offset += 80;
+	      pb += Source_lDelta;
+	    }
+	}
+
+      if (0 != ((i - x) % 2))
+	{
+	  opb++;
+	}
     }
-
-    opb += Source_lDelta;
-
-    pb = opb;
-
-  }
 }
 
 void DIB_TransparentBltToVGA(int x, int y, int w, int h, void *b, int Source_lDelta, ULONG trans)
@@ -470,7 +632,7 @@ void DIB_TransparentBltToVGA(int x, int y, int w, int h, void *b, int Source_lDe
   BYTE  b1, b2;
 
   // Check if the width is odd
-  if(mod(w, 2)>0)
+  if(mod2(w)>0)
   {
     edgePixel = TRUE;
     x2 -= 1;
@@ -500,9 +662,141 @@ void DIB_TransparentBltToVGA(int x, int y, int w, int h, void *b, int Source_lDe
   }
 }
 
+// This algorithm goes from left to right, storing each 4BPP pixel
+// in an entire byte.
+void FASTCALL
+vgaReadScan ( int x, int y, int w, void *b )
+{
+  unsigned char *vp, *vpP;
+  unsigned char data, mask, maskP;
+  unsigned char *bp;
+  unsigned char plane_mask;
+  int byte_per_line = SCREEN_X >> 3;
+  int plane, i;
+
+  ASSIGNVP4(x, y, vpP)
+  ASSIGNMK4(x, y, maskP)
+  get_masks(x, w);
+  WRITE_PORT_USHORT((PUSHORT)GRA_I, 0x0005);  // read mode 0
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x04);  // read map select
+
+  memset ( b, 0, w );
+
+  for ( plane=0, plane_mask=1; plane < 4; plane++, plane_mask<<=1 )
+  {
+    WRITE_PORT_UCHAR((PUCHAR)GRA_D, plane);  // read map select
+
+    vp = vpP;
+    bp = b;
+    if ( leftMask )
+    {
+      mask = maskP;
+      data = *vp++;
+      do
+      {
+	if (data & mask)
+	  *bp |= plane_mask;
+	bp++;
+	mask >>= 1;
+      } while (mask & leftMask);
+
+    }
+    if (byteCounter)
+    {
+      for (i=byteCounter; i>0; i--)
+      {
+	data = *vp++;
+	if (data & 0x80) *bp |= plane_mask;
+	bp++;
+	if (data & 0x40) *bp |= plane_mask;
+	bp++;
+	if (data & 0x20) *bp |= plane_mask;
+	bp++;
+	if (data & 0x10) *bp |= plane_mask;
+	bp++;
+	if (data & 0x08) *bp |= plane_mask;
+	bp++;
+	if (data & 0x04) *bp |= plane_mask;
+	bp++;
+	if (data & 0x02) *bp |= plane_mask;
+	bp++;
+	if (data & 0x01) *bp |= plane_mask;
+	bp++;
+      }
+    }
+    if (rightMask)
+    {
+      mask = 0x80;
+      data = *vp;
+      do
+      {
+	if (data & mask)
+	  *bp |= plane_mask;
+	bp++;
+	mask >>= 1;
+      } while (mask & rightMask);
+    }
+  }
+
+  // We don't need this if the next call is a DFB blt to VGA (as in the case of moving the mouse pointer)
+  //WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
+  //WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
+  //WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
+  //WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
+}
+
+//  This algorithm goes from left to right
+//  It stores each 4BPP pixel in an entire byte.
+void FASTCALL
+vgaWriteScan ( int x, int y, int w, void *b )
+{
+  unsigned char *bp;
+  unsigned char *vp;
+  unsigned char init_mask;
+  volatile unsigned char dummy;
+  int byte_per_line;
+  int i, j, off, init_off = x&7;
+
+  bp = b;
+  ASSIGNVP4(x, y, vp)
+  ASSIGNMK4(x, y, init_mask)
+  byte_per_line = SCREEN_X >> 3;
+
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
+  WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
+  WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);      // bit mask
+
+  //DbgPrint("vgaWriteScan(%i,%i,%i...)\n",x,y,w);
+  for ( j = 0; j < 8; j++ )
+  {
+    unsigned int mask = 0x80 >> j;
+    //DbgPrint("j=%i\n",j);
+    WRITE_PORT_UCHAR ( (PUCHAR)GRA_D, (unsigned char)mask );
+    i = j - init_off;
+    off = 0;
+    if ( j < init_off )
+      i += 8, off++;
+    while ( i < w )
+    {
+      //DbgPrint("(%i)",i);
+      dummy = vp[off];
+      //DbgPrint(".");
+      dummy = bp[i];
+      //DbgPrint(".");
+      vp[off] = dummy;
+      //DbgPrint(".");
+      i += 8;
+      off++;
+    }
+    //DbgPrint("\n");
+  }
+}
+
 void DFB_BltFromVGA(int x, int y, int w, int h, void *b, int bw)
 
-//  This algorithm goes from goes from left to right, and inside that loop, top to bottom.
+//  This algorithm goes from left to right, and inside that loop, top to bottom.
 //  It also stores each 4BPP pixel in an entire byte.
 {
   unsigned char *vp, *vpY, *vpP;
@@ -521,30 +815,38 @@ void DFB_BltFromVGA(int x, int y, int w, int h, void *b, int bw)
 
   // clear buffer
   bp=b;
-  for (j=h; j>0; j--) {
+  for (j=h; j>0; j--)
+  {
     memset(bp, 0, w);
     bp += bw;
   }
 
-  for (plane=0, plane_mask=1; plane<4; plane++, plane_mask<<=1) {
+  for ( plane=0, plane_mask=1; plane < 4; plane++, plane_mask<<=1 )
+  {
     WRITE_PORT_UCHAR((PUCHAR)GRA_D, plane);  // read map select
     vpY = vpP;
     bpY = b;
-    for (j=h; j>0; j--) {
+    for ( j=h; j>0; j-- )
+    {
       vp = vpY;
       bp = bpY;
-      if (leftMask) {
+      if ( leftMask )
+      {
         mask = maskP;
         data = *vp++;
-        do {
-          if (data & mask) *bp |= plane_mask;
+        do
+	{
+          if (data & mask)
+	    *bp |= plane_mask;
           bp++;
           mask >>= 1;
         } while (mask & leftMask);
 
       }
-      if (byteCounter) {
-        for (i=byteCounter; i>0; i--) {
+      if (byteCounter)
+      {
+        for (i=byteCounter; i>0; i--)
+	{
           data = *vp++;
           if (data & 0x80) *bp |= plane_mask;
           bp++;
@@ -564,10 +866,12 @@ void DFB_BltFromVGA(int x, int y, int w, int h, void *b, int bw)
           bp++;
         }
       }
-      if (rightMask) {
+      if (rightMask)
+      {
         mask = 0x80;
         data = *vp;
-        do {
+        do
+	{
           if (data & mask) *bp |= plane_mask;
           bp++;
           mask >>= 1;
@@ -587,7 +891,7 @@ void DFB_BltFromVGA(int x, int y, int w, int h, void *b, int bw)
 
 void DFB_BltToVGA(int x, int y, int w, int h, void *b, int bw)
 
-//  This algorithm goes from goes from left to right, and inside that loop, top to bottom.
+//  This algorithm goes from left to right, and inside that loop, top to bottom.
 //  It also stores each 4BPP pixel in an entire byte.
 {
   unsigned char *bp, *bpX;
@@ -608,18 +912,21 @@ void DFB_BltToVGA(int x, int y, int w, int h, void *b, int bw)
   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);      // bit mask
 
-  for (i=w; i>0; i--) {
+  for (i=w; i>0; i--)
+  {
     WRITE_PORT_UCHAR((PUCHAR)GRA_D, mask);
     bp = bpX;
     vp = vpX;
-    for (j=h; j>0; j--) {
+    for (j=h; j>0; j--)
+    {
       dummy = *vp;
       *vp = *bp;
       bp += bw;
       vp += byte_per_line;
     }
     bpX++;
-    if ((mask >>= 1) == 0) {
+    if ((mask >>= 1) == 0)
+    {
       vpX++;
       mask = 0x80;
     }
@@ -649,11 +956,13 @@ void DFB_BltToVGA_Transparent(int x, int y, int w, int h, void *b, int bw, char
   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);      // bit mask
 
-  for (i=w; i>0; i--) {
+  for (i=w; i>0; i--)
+  {
     WRITE_PORT_UCHAR((PUCHAR)GRA_D, mask);
     bp = bpX;
     vp = vpX;
-    for (j=h; j>0; j--) {
+    for (j=h; j>0; j--)
+    {
       if (*bp != Trans)
       {
         dummy = *vp;
@@ -663,7 +972,8 @@ void DFB_BltToVGA_Transparent(int x, int y, int w, int h, void *b, int bw, char
       vp += byte_per_line;
     }
     bpX++;
-    if ((mask >>= 1) == 0) {
+    if ((mask >>= 1) == 0)
+    {
       vpX++;
       mask = 0x80;
     }
@@ -684,7 +994,7 @@ void DFB_BltToDIB(int x, int y, int w, int h, void *b, int bw, void *bdib, int d
   for (i=w; i>0; i--) {
 
     // determine the bit shift for the DIB pixel
-    dib_shift = mod(w-i, 2);
+    dib_shift = mod2(w-i);
     if(dib_shift > 0) dib_shift = 4;
     dibTmp = dib;
 
@@ -713,7 +1023,7 @@ void DIB_BltToDFB(int x, int y, int w, int h, void *b, int bw, void *bdib, int d
   for (i=w; i>0; i--) {
 
     // determine the bit shift for the DIB pixel
-    dib_shift = mod(w-i, 2);
+    dib_shift = mod2(w-i);
     if(dib_shift > 0) {
       dib_shift = 0;
       dib_and = 0x0f;