drivers/dd/vga/display/vgavideo/vgavideo.c

   1 #include <ddk/ntddk.h>
   2 #include <ddk/ntddvid.h>
   3 #include <ddk/winddi.h>
   4 #include <ntos/minmax.h>
   5 #include "vgavideo.h"
   6
   7 UCHAR PreCalcReverseByte[256];
   8 int maskbit[640];
   9 int y80[480];
  10 int xconv[640];
  11 int bit8[640];
  12 int startmasks[8];
  13 int endmasks[8];
  14 char* vidmem;
  15 static ULONG UnpackPixel[256];
  16
  17 static unsigned char saved_SEQ_mask;    /* 0x02 */
  18 static unsigned char saved_GC_eSR;      /* 0x01 */
  19 static unsigned char saved_GC_fun;      /* 0x03 */
  20 static unsigned char saved_GC_rmap;     /* 0x04 */
  21 static unsigned char saved_GC_mode;     /* 0x05 */
  22 static unsigned char saved_GC_mask;     /* 0x08 */
  23 static unsigned char leftMask;
  24 static int byteCounter;
  25 static unsigned char rightMask;
  26
  27 #define READ_REGISTER_UCHAR(p) (*((PUCHAR)(p)))
  28 #define WRITE_REGISTER_UCHAR(p,c) (*((PCHAR)(p))) = (c)
  29
  30 INT abs(INT nm)
  31 {
  32   if(nm<0)
  33   {
  34     return nm * -1;
  35   } else
  36   {
  37     return nm;
  38   }
  39 }
  40
  41 div_t div(int num, int denom)
  42 {
  43   div_t r;
  44   if (num > 0 && denom < 0) {
  45     num = -num;
  46     denom = -denom;
  47   }
  48   r.quot = num / denom;
  49   r.rem = num % denom;
  50   if (num < 0 && denom > 0)
  51   {
  52     if (r.rem > 0)
  53     {
  54       r.quot++;
  55       r.rem -= denom;
  56     }
  57   }
  58   return r;
  59 }
  60
  61 /*int mod(int num, int denom)
  62 {
  63   div_t dvt = div(num, denom);
  64   return dvt.rem;
  65 }*/
  66
  67 BYTE bytesPerPixel(ULONG Format)
  68 {
  69   // This function is taken from /subsys/win32k/eng/surface.c
  70   // FIXME: GDI bitmaps are supposed to be pixel-packed. Right now if the
  71   // pixel size if < 1 byte we expand it to 1 byte for simplicities sake
  72
  73   switch ( Format )
  74   {
  75   case BMF_1BPP:
  76     return 1;
  77
  78   case BMF_4BPP:
  79   case BMF_4RLE:
  80     return 1;
  81
  82   case BMF_8BPP:
  83   case BMF_8RLE:
  84     return 1;
  85
  86   case BMF_16BPP:
  87     return 2;
  88
  89   case BMF_24BPP:
  90     return 3;
  91
  92   case BMF_32BPP:
  93     return 4;
  94
  95   default:
  96     return 0;
  97   }
  98 }
  99
 100 VOID vgaPreCalc()
 101 {
 102   ULONG j;
 103
 104   startmasks[0] = 255;
 105   startmasks[1] = 1;
 106   startmasks[2] = 3;
 107   startmasks[3] = 7;
 108   startmasks[4] = 15;
 109   startmasks[5] = 31;
 110   startmasks[6] = 63;
 111   startmasks[7] = 127;
 112
 113   endmasks[0] = 0;
 114   endmasks[1] = 128;
 115   endmasks[2] = 192;
 116   endmasks[3] = 224;
 117   endmasks[4] = 240;
 118   endmasks[5] = 248;
 119   endmasks[6] = 252;
 120   endmasks[7] = 254;
 121
 122   for(j=0; j<80; j++)
 123   {
 124     maskbit[j*8]   = 128;
 125     maskbit[j*8+1] = 64;
 126     maskbit[j*8+2] = 32;
 127     maskbit[j*8+3] = 16;
 128     maskbit[j*8+4] = 8;
 129     maskbit[j*8+5] = 4;
 130     maskbit[j*8+6] = 2;
 131     maskbit[j*8+7] = 1;
 132
 133     bit8[j*8]   = 7;
 134     bit8[j*8+1] = 6;
 135     bit8[j*8+2] = 5;
 136     bit8[j*8+3] = 4;
 137     bit8[j*8+4] = 3;
 138     bit8[j*8+5] = 2;
 139     bit8[j*8+6] = 1;
 140     bit8[j*8+7] = 0;
 141   }
 142   for(j=0; j<480; j++)
 143   {
 144     y80[j]  = j*80;
 145   }
 146   for(j=0; j<640; j++)
 147   {
 148     xconv[j] = j >> 3;
 149   }
 150
 151   for (j = 0; j < 256; j++)
 152     {
 153       PreCalcReverseByte[j] =
 154         (((j >> 0) & 0x1) << 7) |
 155         (((j >> 1) & 0x1) << 6) |
 156         (((j >> 2) & 0x1) << 5) |
 157         (((j >> 3) & 0x1) << 4) |
 158         (((j >> 4) & 0x1) << 3) |
 159         (((j >> 5) & 0x1) << 2) |
 160         (((j >> 6) & 0x1) << 1) |
 161         (((j >> 7) & 0x1) << 0);
 162     }
 163
 164   for (j = 0; j < 256; j++)
 165     {
 166       UnpackPixel[j] =
 167         (((j >> 0) & 0x1) << 4) |
 168         (((j >> 1) & 0x1) << 0) |
 169         (((j >> 2) & 0x1) << 12) |
 170         (((j >> 3) & 0x1) << 8) |
 171         (((j >> 4) & 0x1) << 20) |
 172         (((j >> 5) & 0x1) << 16) |
 173         (((j >> 6) & 0x1) << 28) |
 174         (((j >> 7) & 0x1) << 24);
 175     }
 176 }
 177
 178 void
 179 get_masks(int x, int w)
 180 {
 181   register int tmp;
 182
 183   leftMask = rightMask = 0;
 184   byteCounter = w;
 185   /* right margin */
 186   tmp = (x+w) & 7;
 187   if (tmp) {
 188     byteCounter -= tmp;
 189     rightMask = (unsigned char)(0xff00 >> tmp);
 190   }
 191   /* left margin */
 192   tmp = x & 7;
 193   if (tmp) {
 194     byteCounter -= (8 - tmp);
 195     leftMask = (0xff >> tmp);
 196   }
 197   /* too small ? */
 198   if (byteCounter < 0) {
 199     leftMask &= rightMask;
 200     rightMask = 0;
 201     byteCounter = 0;
 202   }
 203   byteCounter /= 8;
 204 }
 205
 206 VOID vgaPutPixel(INT x, INT y, UCHAR c)
 207 {
 208   ULONG offset;
 209   UCHAR a;
 210
 211   offset = xconv[x]+y80[y];
 212
 213   WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);
 214   WRITE_PORT_UCHAR((PUCHAR)GRA_D,maskbit[x]);
 215
 216   a = READ_REGISTER_UCHAR(vidmem + offset);
 217   WRITE_REGISTER_UCHAR(vidmem + offset, c);
 218 }
 219
 220 VOID vgaPutByte(INT x, INT y, UCHAR c)
 221 {
 222   ULONG offset;
 223
 224   offset = xconv[x]+y80[y];
 225
 226   // Set the write mode
 227   WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);
 228   WRITE_PORT_UCHAR((PUCHAR)GRA_D,0xff);
 229
 230   WRITE_REGISTER_UCHAR(vidmem + offset, c);
 231 }
 232
 233 VOID vgaGetByte(ULONG offset,
 234                 UCHAR *b, UCHAR *g,
 235                 UCHAR *r, UCHAR *i)
 236 {
 237   WRITE_PORT_USHORT((PUSHORT)GRA_I, 0x0304);
 238   *i = READ_REGISTER_UCHAR(vidmem + offset);
 239   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
 240   *r = READ_REGISTER_UCHAR(vidmem + offset);
 241   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x01);
 242   *g = READ_REGISTER_UCHAR(vidmem + offset);
 243   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 244   *b = READ_REGISTER_UCHAR(vidmem + offset);
 245 }
 246
 247 INT vgaGetPixel(INT x, INT y)
 248 {
 249   UCHAR mask, b, g, r, i;
 250   ULONG offset;
 251
 252   offset = xconv[x]+y80[y];
 253   vgaGetByte(offset, &b, &g, &r, &i);
 254
 255   mask=maskbit[x];
 256   b=b&mask;
 257   g=g&mask;
 258   r=r&mask;
 259   i=i&mask;
 260
 261   mask=bit8[x];
 262   g=g>>mask;
 263   b=b>>mask;
 264   r=r>>mask;
 265   i=i>>mask;
 266
 267   return(b+2*g+4*r+8*i);
 268 }
 269
 270 BOOL vgaHLine(INT x, INT y, INT len, UCHAR c)
 271 {
 272   UCHAR a;
 273   ULONG pre1, i;
 274   ULONG orgpre1, orgx, midpre1;
 275   ULONG ileftpix, imidpix, irightpix;
 276
 277   orgx = x;
 278
 279   /*if ( len < 8 )
 280   {
 281     for (i = x; i < x+len; i++ )
 282       vgaPutPixel ( i, y, c );
 283
 284     return TRUE;
 285   }*/
 286
 287   // Calculate the left mask pixels, middle bytes and right mask pixel
 288   ileftpix = 7 - mod8(x-1);
 289   irightpix = mod8(x+len);
 290   imidpix = (len-ileftpix-irightpix) / 8;
 291
 292   pre1 = xconv[(x-1)&~7] + y80[y];
 293   orgpre1=pre1;
 294
 295   // check for overlap ( very short line )
 296   if ( (ileftpix+irightpix) > len )
 297   {
 298     int mask = startmasks[ileftpix] & endmasks[irightpix];
 299     // Write left pixels
 300     WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);     // set the mask
 301     WRITE_PORT_UCHAR((PUCHAR)GRA_D,mask);
 302
 303     a = READ_REGISTER_UCHAR(vidmem + pre1);
 304     WRITE_REGISTER_UCHAR(vidmem + pre1, c);
 305
 306     return TRUE;
 307   }
 308
 309   // Left
 310   if ( ileftpix > 0 )
 311   {
 312     // Write left pixels
 313     WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);     // set the mask
 314     WRITE_PORT_UCHAR((PUCHAR)GRA_D,startmasks[ileftpix]);
 315
 316     a = READ_REGISTER_UCHAR(vidmem + pre1);
 317     WRITE_REGISTER_UCHAR(vidmem + pre1, c);
 318
 319     // Prepare new x for the middle
 320     x = orgx + 8;
 321   }
 322
 323   if ( imidpix > 0 )
 324   {
 325     midpre1 = xconv[x] + y80[y];
 326
 327     // Set mask to all pixels in byte
 328     WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);
 329     WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0xff);
 330     memset(vidmem+midpre1, c, imidpix); // write middle pixels, no need to read in latch because of the width
 331   }
 332
 333   if ( irightpix > 0 )
 334   {
 335     x = orgx + len - irightpix;
 336     pre1 = xconv[x] + y80[y];
 337
 338     // Write right pixels
 339     WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);     // set the mask bits
 340     WRITE_PORT_UCHAR((PUCHAR)GRA_D, endmasks[irightpix]);
 341     a = READ_REGISTER_UCHAR(vidmem + pre1);
 342     WRITE_REGISTER_UCHAR(vidmem + pre1, c);
 343   }
 344
 345   return TRUE;
 346 }
 347
 348 BOOL vgaVLine(INT x, INT y, INT len, UCHAR c)
 349 {
 350   ULONG offset, i;
 351   UCHAR a;
 352
 353   offset = xconv[x]+y80[y];
 354
 355 #ifdef VGA_PERF
 356   vgaSetBitMaskRegister ( maskbit[x] );
 357 #else
 358   WRITE_PORT_UCHAR((PUCHAR)GRA_I,0x08);       // set the mask
 359   WRITE_PORT_UCHAR((PUCHAR)GRA_D,maskbit[x]);
 360 #endif
 361
 362   for(i=y; i<y+len; i++)
 363   {
 364     a = READ_REGISTER_UCHAR(vidmem + offset);
 365     WRITE_REGISTER_UCHAR(vidmem + offset, c);
 366     offset+=80;
 367   }
 368
 369   return TRUE;
 370 }
 371
 372 static const RECTL rclEmpty = { 0, 0, 0, 0 };
 373
 374 BOOL VGADDIIntersectRect(PRECTL prcDst, PRECTL prcSrc1, PRECTL prcSrc2)
 375 {
 376   prcDst->left  = max(prcSrc1->left, prcSrc2->left);
 377   prcDst->right = min(prcSrc1->right, prcSrc2->right);
 378
 379   if (prcDst->left < prcDst->right) {
 380       prcDst->top = max(prcSrc1->top, prcSrc2->top);
 381       prcDst->bottom = min(prcSrc1->bottom, prcSrc2->bottom);
 382
 383      if (prcDst->top < prcDst->bottom)
 384      {
 385        return TRUE;
 386      }
 387   }
 388
 389   *prcDst = rclEmpty;
 390
 391   return FALSE;
 392 }
 393
 394 void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
 395 {
 396   ULONG plane;
 397   ULONG left = x >> 3;
 398   ULONG shift = x - (x & ~0x7);
 399   UCHAR pixel, nextpixel;
 400   ULONG rightcount;
 401   ULONG i, j;
 402   ULONG stride = w >> 3;
 403
 404   /* Calculate the number of rightmost bytes not in a dword block. */
 405   if (w >= 8)
 406     {
 407       rightcount = w % 8;
 408     }
 409   else
 410     {
 411       stride = 0;
 412       rightcount = w;
 413     }
 414
 415   /* Reset the destination. */
 416   memset(b, 0, h * Dest_lDelta);
 417
 418   for (plane = 0; plane < 4; plane++)
 419     {
 420       PUCHAR dest = b;
 421
 422       /* Select the plane we are reading in this iteration. */
 423       WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x04);
 424       WRITE_PORT_UCHAR((PUCHAR)GRA_D, plane);
 425
 426       for (j = 0; j < h; j++)
 427         {
 428           PULONG destline = (PULONG)dest;
 429           PUCHAR src = vidmem + (y + j) * SCREEN_STRIDE + left;
 430           /* Read the data for one plane for an eight aligned pixel block. */
 431           nextpixel = PreCalcReverseByte[READ_REGISTER_UCHAR(src)];
 432           for (i = 0; i < stride; i++, src++, destline++)
 433             {
 434               /* Form the data for one plane for an aligned block in the destination. */
 435               pixel = nextpixel;
 436               pixel >>= shift;
 437
 438               nextpixel = PreCalcReverseByte[READ_REGISTER_UCHAR(src + 1)];
 439               pixel |= (nextpixel << (8 - shift));
 440
 441               /* Expand the plane data to 'chunky' format and store. */
 442               *destline |= (UnpackPixel[pixel] << plane);
 443             }
 444           /* Handle any pixels not falling into a full block. */
 445           if (rightcount != 0)
 446             {
 447               ULONG row;
 448
 449               /* Form the data for a complete block. */
 450               pixel = nextpixel;
 451               pixel >>= shift;
 452
 453               nextpixel = PreCalcReverseByte[READ_REGISTER_UCHAR(src + 1)];
 454               pixel |= (nextpixel << (8 - shift));
 455
 456               row = UnpackPixel[pixel] << plane;
 457
 458               /* Store the data for each pixel in the destination. */
 459               for (i = 0; i < rightcount; i++)
 460                 {
 461                   ((PUCHAR)destline)[i] |= (row & 0xFF);
 462                   row >>= 8;
 463                 }
 464             }
 465           dest += Dest_lDelta;
 466         }
 467     }
 468
 469 #ifdef VGA_VERIFY
 470   for (j = 0; j < h; j++)
 471     {
 472       for (i = 0; i < w; i+=2)
 473         {
 474           UCHAR c1, c2;
 475           ULONG mask = (i < (w - 1)) ? 0xFF : 0xF0;
 476
 477           c1 = (vgaGetPixel(x + i, y + j) << 4) | (vgaGetPixel(x + i + 1, y + j));
 478           c2 = ((PUCHAR)b)[(j * Dest_lDelta) + (i >> 1)];
 479           if ((c1 & mask) != (c2 & mask))
 480             {
 481               __asm__("int $3\n\t" : /* no outputs */ : /* no inputs */);
 482             }
 483         }
 484     }
 485 #endif /* VGA_VERIFY */
 486 }
 487
 488 #if 0
 489 void DIB_BltFromVGA(int x, int y, int w, int h, void *b, int Dest_lDelta)
 490 //  DIB blt from the VGA.
 491 //  For now we just do slow reads -- pixel by pixel, packing each one into the correct 4BPP format.
 492 {
 493   PBYTE  pb = b, opb = b;
 494   BOOLEAN  edgePixel = FALSE;
 495   ULONG  i, j;
 496   ULONG  x2 = x + w;
 497   ULONG  y2 = y + h;
 498   BYTE  b1, b2;
 499
 500   // Check if the width is odd
 501   if(mod2(w)>0)
 502   {
 503     edgePixel = TRUE;
 504     x2 -= 1;
 505   }
 506
 507   for (j=y; j<y2; j++)
 508   {
 509     for (i=x; i<x2; i+=2)
 510     {
 511       b1 = vgaGetPixel(i,  j);
 512       b2 = vgaGetPixel(i+1,  j);
 513       *pb = b2 | (b1 << 4);
 514       pb++;
 515     }
 516
 517     if(edgePixel == TRUE)
 518     {
 519       b1 = vgaGetPixel(x2, j);
 520       *pb = b1 << 4;
 521       pb++;
 522     }
 523
 524     opb += Dest_lDelta; // new test code
 525     pb = opb; // new test code
 526   }
 527 }
 528 #endif
 529
 530 /* DIB blt to the VGA. */
 531 void DIB_BltToVGA(int x, int y, int w, int h, void *b, int Source_lDelta)
 532 {
 533   PBYTE pb, opb = b;
 534   ULONG i, j;
 535   ULONG x2 = x + w;
 536   ULONG y2 = y + h;
 537   ULONG offset;
 538   UCHAR a;
 539
 540   for (i = x; i < x2; i++)
 541     {
 542       pb = opb;
 543       offset = xconv[i] + y80[y];
 544
 545       WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);       // set the mask
 546       WRITE_PORT_UCHAR((PUCHAR)GRA_D, maskbit[i]);
 547
 548       if (0 == ((i - x) % 2))
 549         {
 550           for (j = y; j < y2; j++)
 551             {
 552               a = READ_REGISTER_UCHAR(vidmem + offset);
 553               WRITE_REGISTER_UCHAR(vidmem + offset, (*pb & 0xf0) >> 4);
 554               offset += 80;
 555               pb += Source_lDelta;
 556             }
 557         }
 558       else
 559         {
 560           for (j = y; j < y2; j++)
 561             {
 562               a = READ_REGISTER_UCHAR(vidmem + offset);
 563               WRITE_REGISTER_UCHAR(vidmem + offset, *pb & 0x0f);
 564               offset += 80;
 565               pb += Source_lDelta;
 566             }
 567         }
 568
 569       if (0 != ((i - x) % 2))
 570         {
 571           opb++;
 572         }
 573     }
 574 }
 575
 576 /* DIB blt to the VGA. */
 577 void DIB_BltToVGAWithXlate(int x, int y, int w, int h, void *b, int Source_lDelta, PXLATEOBJ Xlate)
 578 {
 579   PBYTE pb, opb = b;
 580   ULONG i, j;
 581   ULONG x2 = x + w;
 582   ULONG y2 = y + h;
 583   ULONG offset;
 584   UCHAR a;
 585
 586   for (i = x; i < x2; i++)
 587     {
 588       pb = opb;
 589       offset = xconv[i] + y80[y];
 590
 591       WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);       // set the mask
 592       WRITE_PORT_UCHAR((PUCHAR)GRA_D, maskbit[i]);
 593
 594       if (0 == ((i - x) % 2))
 595         {
 596           for (j = y; j < y2; j++)
 597             {
 598               a = READ_REGISTER_UCHAR(vidmem + offset);
 599               WRITE_REGISTER_UCHAR(vidmem + offset, XLATEOBJ_iXlate(Xlate, (*pb & 0xf0) >> 4));
 600               offset += 80;
 601               pb += Source_lDelta;
 602             }
 603         }
 604       else
 605         {
 606           for (j = y; j < y2; j++)
 607             {
 608               a = READ_REGISTER_UCHAR(vidmem + offset);
 609               WRITE_REGISTER_UCHAR(vidmem + offset, XLATEOBJ_iXlate(Xlate, *pb & 0x0f));
 610               offset += 80;
 611               pb += Source_lDelta;
 612             }
 613         }
 614
 615       if (0 != ((i - x) % 2))
 616         {
 617           opb++;
 618         }
 619     }
 620 }
 621
 622 void DIB_TransparentBltToVGA(int x, int y, int w, int h, void *b, int Source_lDelta, ULONG trans)
 623
 624 //  DIB blt to the VGA.
 625 //  For now we just do slow writes -- pixel by pixel, packing each one into the correct 4BPP format.
 626 {
 627   PBYTE  pb = b, opb = b;
 628   BOOLEAN  edgePixel = FALSE;
 629   ULONG  i, j;
 630   ULONG  x2 = x + w;
 631   ULONG  y2 = y + h;
 632   BYTE  b1, b2;
 633
 634   // Check if the width is odd
 635   if(mod2(w)>0)
 636   {
 637     edgePixel = TRUE;
 638     x2 -= 1;
 639   }
 640
 641   for (j=y; j<y2; j++)
 642   {
 643     for (i=x; i<x2; i+=2)
 644     {
 645       b1 = (*pb & 0xf0) >> 4;
 646       b2 = *pb & 0x0f;
 647       if(b1 != trans) vgaPutPixel(i,   j, b1);
 648       if(b2 != trans) vgaPutPixel(i+1, j, b2);
 649       pb++;
 650     }
 651
 652     if(edgePixel == TRUE)
 653     {
 654       b1 = *pb;
 655       if(b1 != trans) vgaPutPixel(x2, j, b1);
 656       pb++;
 657     }
 658
 659     opb += Source_lDelta;
 660     pb = opb; // new test code
 661
 662   }
 663 }
 664
 665 // This algorithm goes from left to right, storing each 4BPP pixel
 666 // in an entire byte.
 667 void FASTCALL
 668 vgaReadScan ( int x, int y, int w, void *b )
 669 {
 670   unsigned char *vp, *vpP;
 671   unsigned char data, mask, maskP;
 672   unsigned char *bp;
 673   unsigned char plane_mask;
 674   int byte_per_line = SCREEN_X >> 3;
 675   int plane, i;
 676
 677   ASSIGNVP4(x, y, vpP)
 678   ASSIGNMK4(x, y, maskP)
 679   get_masks(x, w);
 680   WRITE_PORT_USHORT((PUSHORT)GRA_I, 0x0005);  // read mode 0
 681   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x04);  // read map select
 682
 683   memset ( b, 0, w );
 684
 685   for ( plane=0, plane_mask=1; plane < 4; plane++, plane_mask<<=1 )
 686   {
 687     WRITE_PORT_UCHAR((PUCHAR)GRA_D, plane);  // read map select
 688
 689     vp = vpP;
 690     bp = b;
 691     if ( leftMask )
 692     {
 693       mask = maskP;
 694       data = *vp++;
 695       do
 696       {
 697         if (data & mask)
 698           *bp |= plane_mask;
 699         bp++;
 700         mask >>= 1;
 701       } while (mask & leftMask);
 702
 703     }
 704     if (byteCounter)
 705     {
 706       for (i=byteCounter; i>0; i--)
 707       {
 708         data = *vp++;
 709         if (data & 0x80) *bp |= plane_mask;
 710         bp++;
 711         if (data & 0x40) *bp |= plane_mask;
 712         bp++;
 713         if (data & 0x20) *bp |= plane_mask;
 714         bp++;
 715         if (data & 0x10) *bp |= plane_mask;
 716         bp++;
 717         if (data & 0x08) *bp |= plane_mask;
 718         bp++;
 719         if (data & 0x04) *bp |= plane_mask;
 720         bp++;
 721         if (data & 0x02) *bp |= plane_mask;
 722         bp++;
 723         if (data & 0x01) *bp |= plane_mask;
 724         bp++;
 725       }
 726     }
 727     if (rightMask)
 728     {
 729       mask = 0x80;
 730       data = *vp;
 731       do
 732       {
 733         if (data & mask)
 734           *bp |= plane_mask;
 735         bp++;
 736         mask >>= 1;
 737       } while (mask & rightMask);
 738     }
 739   }
 740
 741   // We don't need this if the next call is a DFB blt to VGA (as in the case of moving the mouse pointer)
 742   //WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
 743   //WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
 744   //WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
 745   //WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 746 }
 747
 748 //  This algorithm goes from left to right
 749 //  It stores each 4BPP pixel in an entire byte.
 750 void FASTCALL
 751 vgaWriteScan ( int x, int y, int w, void *b )
 752 {
 753   unsigned char *bp;
 754   unsigned char *vp;
 755   unsigned char init_mask;
 756   volatile unsigned char dummy;
 757   int byte_per_line;
 758   int i, j, off, init_off = x&7;
 759
 760   bp = b;
 761   ASSIGNVP4(x, y, vp)
 762   ASSIGNMK4(x, y, init_mask)
 763   byte_per_line = SCREEN_X >> 3;
 764
 765   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
 766   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
 767   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
 768   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 769   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);      // bit mask
 770
 771   //DbgPrint("vgaWriteScan(%i,%i,%i...)\n",x,y,w);
 772   for ( j = 0; j < 8; j++ )
 773   {
 774     unsigned int mask = 0x80 >> j;
 775     //DbgPrint("j=%i\n",j);
 776     WRITE_PORT_UCHAR ( (PUCHAR)GRA_D, (unsigned char)mask );
 777     i = j - init_off;
 778     off = 0;
 779     if ( j < init_off )
 780       i += 8, off++;
 781     while ( i < w )
 782     {
 783       //DbgPrint("(%i)",i);
 784       dummy = vp[off];
 785       //DbgPrint(".");
 786       dummy = bp[i];
 787       //DbgPrint(".");
 788       vp[off] = dummy;
 789       //DbgPrint(".");
 790       i += 8;
 791       off++;
 792     }
 793     //DbgPrint("\n");
 794   }
 795 }
 796
 797 void DFB_BltFromVGA(int x, int y, int w, int h, void *b, int bw)
 798
 799 //  This algorithm goes from left to right, and inside that loop, top to bottom.
 800 //  It also stores each 4BPP pixel in an entire byte.
 801 {
 802   unsigned char *vp, *vpY, *vpP;
 803   unsigned char data, mask, maskP;
 804   unsigned char *bp, *bpY;
 805   unsigned char plane_mask;
 806   int byte_per_line = SCREEN_X >> 3;
 807   int plane, i, j;
 808
 809   ASSIGNVP4(x, y, vpP)
 810   ASSIGNMK4(x, y, maskP)
 811   get_masks(x, w);
 812   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);  // read mode 0
 813   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 814   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x04);  // read map select
 815
 816   // clear buffer
 817   bp=b;
 818   for (j=h; j>0; j--)
 819   {
 820     memset(bp, 0, w);
 821     bp += bw;
 822   }
 823
 824   for ( plane=0, plane_mask=1; plane < 4; plane++, plane_mask<<=1 )
 825   {
 826     WRITE_PORT_UCHAR((PUCHAR)GRA_D, plane);  // read map select
 827     vpY = vpP;
 828     bpY = b;
 829     for ( j=h; j>0; j-- )
 830     {
 831       vp = vpY;
 832       bp = bpY;
 833       if ( leftMask )
 834       {
 835         mask = maskP;
 836         data = *vp++;
 837         do
 838         {
 839           if (data & mask)
 840             *bp |= plane_mask;
 841           bp++;
 842           mask >>= 1;
 843         } while (mask & leftMask);
 844
 845       }
 846       if (byteCounter)
 847       {
 848         for (i=byteCounter; i>0; i--)
 849         {
 850           data = *vp++;
 851           if (data & 0x80) *bp |= plane_mask;
 852           bp++;
 853           if (data & 0x40) *bp |= plane_mask;
 854           bp++;
 855           if (data & 0x20) *bp |= plane_mask;
 856           bp++;
 857           if (data & 0x10) *bp |= plane_mask;
 858           bp++;
 859           if (data & 0x08) *bp |= plane_mask;
 860           bp++;
 861           if (data & 0x04) *bp |= plane_mask;
 862           bp++;
 863           if (data & 0x02) *bp |= plane_mask;
 864           bp++;
 865           if (data & 0x01) *bp |= plane_mask;
 866           bp++;
 867         }
 868       }
 869       if (rightMask)
 870       {
 871         mask = 0x80;
 872         data = *vp;
 873         do
 874         {
 875           if (data & mask) *bp |= plane_mask;
 876           bp++;
 877           mask >>= 1;
 878         } while (mask & rightMask);
 879       }
 880       bpY += bw;
 881       vpY += byte_per_line;
 882     }
 883   }
 884
 885   // We don't need this if the next call is a DFB blt to VGA (as in the case of moving the mouse pointer)
 886   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
 887   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
 888   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
 889   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 890 }
 891
 892 void DFB_BltToVGA(int x, int y, int w, int h, void *b, int bw)
 893
 894 //  This algorithm goes from left to right, and inside that loop, top to bottom.
 895 //  It also stores each 4BPP pixel in an entire byte.
 896 {
 897   unsigned char *bp, *bpX;
 898   unsigned char *vp, *vpX;
 899   unsigned char mask;
 900   volatile unsigned char dummy;
 901   int byte_per_line;
 902   int i, j;
 903
 904   bpX = b;
 905   ASSIGNVP4(x, y, vpX)
 906   ASSIGNMK4(x, y, mask)
 907   byte_per_line = SCREEN_X >> 3;
 908
 909   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
 910   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
 911   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
 912   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 913   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);      // bit mask
 914
 915   for (i=w; i>0; i--)
 916   {
 917     WRITE_PORT_UCHAR((PUCHAR)GRA_D, mask);
 918     bp = bpX;
 919     vp = vpX;
 920     for (j=h; j>0; j--)
 921     {
 922       dummy = *vp;
 923       *vp = *bp;
 924       bp += bw;
 925       vp += byte_per_line;
 926     }
 927     bpX++;
 928     if ((mask >>= 1) == 0)
 929     {
 930       vpX++;
 931       mask = 0x80;
 932     }
 933   }
 934 }
 935
 936 void DFB_BltToVGA_Transparent(int x, int y, int w, int h, void *b, int bw, char Trans)
 937
 938 //  This algorithm goes from goes from left to right, and inside that loop, top to bottom.
 939 //  It also stores each 4BPP pixel in an entire byte.
 940 {
 941   unsigned char *bp, *bpX;
 942   unsigned char *vp, *vpX;
 943   unsigned char mask;
 944   volatile unsigned char dummy;
 945   int byte_per_line;
 946   int i, j;
 947
 948   bpX = b;
 949   ASSIGNVP4(x, y, vpX)
 950   ASSIGNMK4(x, y, mask)
 951   byte_per_line = SCREEN_X >> 3;
 952
 953   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x05);      // write mode 2
 954   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x02);
 955   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x03);      // replace
 956   WRITE_PORT_UCHAR((PUCHAR)GRA_D, 0x00);
 957   WRITE_PORT_UCHAR((PUCHAR)GRA_I, 0x08);      // bit mask
 958
 959   for (i=w; i>0; i--)
 960   {
 961     WRITE_PORT_UCHAR((PUCHAR)GRA_D, mask);
 962     bp = bpX;
 963     vp = vpX;
 964     for (j=h; j>0; j--)
 965     {
 966       if (*bp != Trans)
 967       {
 968         dummy = *vp;
 969         *vp = *bp;
 970       }
 971       bp += bw;
 972       vp += byte_per_line;
 973     }
 974     bpX++;
 975     if ((mask >>= 1) == 0)
 976     {
 977       vpX++;
 978       mask = 0x80;
 979     }
 980   }
 981 }
 982
 983 void DFB_BltToDIB(int x, int y, int w, int h, void *b, int bw, void *bdib, int dibw)
 984
 985 // This algorithm converts a DFB into a DIB
 986 // WARNING: This algorithm is buggy
 987 {
 988   unsigned char *bp, *bpX, *dib, *dibTmp;
 989   int i, j, dib_shift;
 990
 991   bpX = b;
 992   dib = bdib + y * dibw + (x / 2);
 993
 994   for (i=w; i>0; i--) {
 995
 996     // determine the bit shift for the DIB pixel
 997     dib_shift = mod2(w-i);
 998     if(dib_shift > 0) dib_shift = 4;
 999     dibTmp = dib;
1000
1001     bp = bpX;
1002     for (j=h; j>0; j--) {
1003       *dibTmp = *bp << dib_shift | *(bp + 1);
1004       dibTmp += dibw;
1005       bp += bw;
1006     }
1007     bpX++;
1008     if(dib_shift == 0) dib++;
1009   }
1010 }
1011
1012
1013 void DIB_BltToDFB(int x, int y, int w, int h, void *b, int bw, void *bdib, int dibw)
1014
1015 // This algorithm converts a DIB into a DFB
1016 {
1017   unsigned char *bp, *bpX, *dib, *dibTmp;
1018   int i, j, dib_shift, dib_and;
1019
1020   bpX = b;
1021   dib = bdib + y * dibw + (x / 2);
1022
1023   for (i=w; i>0; i--) {
1024
1025     // determine the bit shift for the DIB pixel
1026     dib_shift = mod2(w-i);
1027     if(dib_shift > 0) {
1028       dib_shift = 0;
1029       dib_and = 0x0f;
1030     } else {
1031       dib_shift = 4;
1032       dib_and = 0xf0;
1033     }
1034
1035     dibTmp = dib;
1036     bp = bpX;
1037
1038     for (j=h; j>0; j--) {
1039       *bp = (*dibTmp & dib_and) >> dib_shift;
1040       dibTmp += dibw;
1041       bp += bw;
1042     }
1043
1044     bpX++;
1045     if(dib_shift == 0) dib++;
1046   }
1047 }