KernelLand/Kernel/drvutil.c

   1 /*
   2  * Acess2 Kernel
   3  * - By John Hodge
   4  *
   5  * drvutil.c
   6  * - Common Driver/Filesystem Helper Functions
   7  */
   8 #define DEBUG   0
   9 #include <acess.h>
  10 #include <api_drv_disk.h>
  11 #include <api_drv_video.h>
  12
  13 // === TYPES ===
  14
  15 // === PROTOTYPES ===
  16 //int   DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length, tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers);
  17 //size_t        DrvUtil_Video_WriteLFB(int Mode, tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, void *Src);
  18 //void  DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap);
  19 //void  DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y);
  20 void    DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf);
  21 //void  DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf);
  22 void    DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour);
  23 void    DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H);
  24
  25 // === GLOBALS ===
  26 tDrvUtil_Video_2DHandlers       gDrvUtil_Stub_2DFunctions = {
  27         NULL,
  28         DrvUtil_Video_2D_Fill,
  29         DrvUtil_Video_2D_Blit
  30 };
  31 tVideo_IOCtl_Bitmap     gDrvUtil_TextModeCursor = {
  32         8, 16,
  33         0, 0,
  34         {
  35                 0, 0, 0         , 0, 0, 0, 0, 0,
  36                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  37                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  38                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  39                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  40                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  41                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  42                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  43                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  44                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  45                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  46                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  47                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  48                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
  49                 0, 0, 0         , 0, 0, 0, 0, 0,
  50                 0, 0, 0         , 0, 0, 0, 0, 0
  51         }
  52 };
  53
  54 // === CODE ===
  55 // --- Video Driver Helpers ---
  56 int DrvUtil_Video_2DStream(void *Ent, const void *Buffer, int Length,
  57         tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers)
  58 {
  59         const Uint8     *stream = Buffer;
  60          int    rem = Length;
  61          int    op;
  62         while( rem )
  63         {
  64                 rem --;
  65                 op = *stream;
  66                 stream ++;
  67
  68                 if(op > NUM_VIDEO_2DOPS) {
  69                         Log_Warning("DrvUtil",
  70                                 "DrvUtil_Video_2DStream: Unknown operation %i",
  71                                 op);
  72                         return Length-rem;
  73                 }
  74
  75                 if(op*sizeof(void*) > SizeofHandlers) {
  76                         Log_Warning("DrvUtil",
  77                                 "DrvUtil_Video_2DStream: Driver does not support op %i",
  78                                 op);
  79                         return Length-rem;
  80                 }
  81
  82                 switch(op)
  83                 {
  84                 case VIDEO_2DOP_NOP:    break;
  85
  86                 case VIDEO_2DOP_FILL:
  87                         if(rem < 10)    return Length-rem;
  88
  89                         if(!Handlers->Fill) {
  90                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
  91                                         " does not support VIDEO_2DOP_FILL");
  92                                 return Length-rem;
  93                         }
  94
  95                         Handlers->Fill(
  96                                 Ent,
  97                                 ((const Uint16*)stream)[0], ((const Uint16*)stream)[1],
  98                                 ((const Uint16*)stream)[2], ((const Uint16*)stream)[3],
  99                                 ((const Uint32*)stream)[4]
 100                                 );
 101
 102                         rem -= 10;
 103                         stream += 10;
 104                         break;
 105
 106                 case VIDEO_2DOP_BLIT:
 107                         if(rem < 12)    return Length-rem;
 108
 109                         if(!Handlers->Blit) {
 110                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
 111                                         " does not support VIDEO_2DOP_BLIT");
 112                                 return Length-rem;
 113                         }
 114
 115                         Handlers->Blit(
 116                                 Ent,
 117                                 ((const Uint16*)stream)[0], ((const Uint16*)stream)[1],
 118                                 ((const Uint16*)stream)[2], ((const Uint16*)stream)[3],
 119                                 ((const Uint16*)stream)[4], ((const Uint16*)stream)[5]
 120                                 );
 121
 122                         rem -= 12;
 123                         stream += 12;
 124                         break;
 125
 126                 }
 127         }
 128         return 0;
 129 }
 130
 131 int DrvUtil_Video_WriteLFB(tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, const void *Buffer)
 132 {
 133         Uint8   *dest;
 134         const Uint32    *src = Buffer;
 135          int    csr_x, csr_y;
 136          int    x, y;
 137          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 138         ENTER("pFBInfo xOffset xLength pBuffer",
 139                 FBInfo, Offset, Length, Buffer);
 140
 141         csr_x = FBInfo->CursorX;
 142         csr_y = FBInfo->CursorY;
 143
 144         DrvUtil_Video_RemoveCursor(FBInfo);
 145
 146         switch( FBInfo->BufferFormat )
 147         {
 148         case VIDEO_BUFFMT_TEXT:
 149                 {
 150                 const tVT_Char  *chars = Buffer;
 151                  int    widthInChars = FBInfo->Width/giVT_CharWidth;
 152                  int    heightInChars = FBInfo->Height/giVT_CharHeight;
 153                  int    i;
 154
 155                 LOG("bytes_per_px = %i", bytes_per_px);
 156                 LOG("widthInChars = %i, heightInChars = %i", widthInChars, heightInChars);
 157
 158                 Length /= sizeof(tVT_Char);     Offset /= sizeof(tVT_Char);
 159
 160                 x = Offset % widthInChars;      y = Offset / widthInChars;
 161                 LOG("x = %i, y = %i", x, y);
 162
 163                 // Sanity Check
 164                 if(Offset > heightInChars * widthInChars)       LEAVE_RET('i', 0);
 165                 if(y >= heightInChars)  LEAVE_RET('i', 0);
 166
 167                 if( Offset + Length > heightInChars*widthInChars )
 168                 {
 169                         Length = heightInChars*widthInChars - Offset;
 170                 }
 171
 172                 dest = FBInfo->Framebuffer;
 173                 LOG("dest = %p", dest);
 174                 dest += y * giVT_CharHeight * FBInfo->Pitch;
 175                 LOG("dest = %p", dest);
 176
 177                 for( i = 0; i < Length; i++ )
 178                 {
 179                         if( y >= heightInChars )
 180                         {
 181                                 Log_Notice("DrvUtil", "Stopped at %i", i);
 182                                 break;
 183                         }
 184
 185                         VT_Font_Render(
 186                                 chars->Ch,
 187                                 dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch,
 188                                 VT_Colour12toN(chars->BGCol, FBInfo->Depth),
 189                                 VT_Colour12toN(chars->FGCol, FBInfo->Depth)
 190                                 );
 191
 192                         chars ++;
 193                         x ++;
 194                         if( x >= widthInChars )
 195                         {
 196                                 x = 0;
 197                                 y ++;
 198                                 dest += FBInfo->Pitch*giVT_CharHeight;
 199                                 LOG("dest = %p", dest);
 200                         }
 201                 }
 202                 Length = i * sizeof(tVT_Char);
 203                 }
 204                 break;
 205
 206         case VIDEO_BUFFMT_FRAMEBUFFER:
 207                 if(FBInfo->Width*FBInfo->Height*4 < Offset+Length)
 208                 {
 209                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow");
 210                         return 0;
 211                 }
 212
 213                 switch(FBInfo->Depth)
 214                 {
 215                 case 15:
 216                 case 16:
 217                         Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in LFB write");
 218                         break;
 219                 case 24:
 220                         x = Offset % FBInfo->Width;
 221                         y = Offset / FBInfo->Width;
 222                         dest = (Uint8*)FBInfo->Framebuffer + y*FBInfo->Pitch;
 223                         for( ; Length >= 4; Length -= 4 )
 224                         {
 225                                 dest[x*3+0] = *src & 0xFF;
 226                                 dest[x*3+1] = (*src >> 8) & 0xFF;
 227                                 dest[x*3+2] = (*src >> 16) & 0xFF;
 228                                 x ++;
 229                                 if(x == FBInfo->Width) {
 230                                         dest += FBInfo->Pitch;
 231                                         x = 0;
 232                                 }
 233                         }
 234                         break;
 235                 case 32:
 236                         // Copy to Frambuffer
 237                         if( FBInfo->Pitch != FBInfo->Width*4 )
 238                         {
 239                                 Uint32  *px;
 240                                 // Pitch isn't 4*Width
 241                                 x = Offset % FBInfo->Width;
 242                                 y = Offset / FBInfo->Height;
 243
 244                                 px = (Uint32*)FBInfo->Framebuffer + y*FBInfo->Pitch/4;
 245
 246                                 for( ; Length >= 4; Length -= 4, x )
 247                                 {
 248                                         px[x++] = *src ++;
 249                                         if( x == FBInfo->Width ) {
 250                                                 x = 0;
 251                                                 px += FBInfo->Pitch;
 252                                         }
 253                                 }
 254                         }
 255                         else
 256                         {
 257                                 dest = (Uint8 *)FBInfo->Framebuffer + Offset;
 258                                 memcpy(dest, Buffer, Length);
 259                         }
 260                         break;
 261                 default:
 262                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Unknown bit depthn %i", FBInfo->Depth);
 263                         break;
 264                 }
 265                 break;
 266
 267         case VIDEO_BUFFMT_2DSTREAM:
 268                 Length = DrvUtil_Video_2DStream(
 269                         FBInfo, Buffer, Length,
 270                         &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions)
 271                         );
 272                 break;
 273
 274         default:
 275                 LEAVE('i', -1);
 276                 return -1;
 277         }
 278
 279         DrvUtil_Video_DrawCursor(FBInfo, csr_x, csr_y);
 280
 281         LEAVE('x', Length);
 282         return Length;
 283 }
 284
 285 int DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap)
 286 {
 287          int    csrX = Buf->CursorX, csrY = Buf->CursorY;
 288         size_t  size;
 289
 290         ENTER("pBuf pBitmap", Buf, Bitmap);
 291
 292         // Clear old bitmap
 293         if( Buf->CursorBitmap )
 294         {
 295                 LOG("Clearing old cursor");
 296                 DrvUtil_Video_RemoveCursor(Buf);
 297                 if( !Bitmap || Bitmap->W != Buf->CursorBitmap->W || Bitmap->H != Buf->CursorBitmap->H )
 298                 {
 299                         free( Buf->CursorSaveBuf );
 300                         Buf->CursorSaveBuf = NULL;
 301                 }
 302                 if( Buf->CursorBitmap != &gDrvUtil_TextModeCursor)
 303                         free(Buf->CursorBitmap);
 304                 Buf->CursorBitmap = NULL;
 305         }
 306
 307         // If the new bitmap is null, disable drawing
 308         if( !Bitmap )
 309         {
 310                 Buf->CursorX = -1;
 311                 Buf->CursorY = -1;
 312                 LEAVE('i', 0);
 313                 return 0;
 314         }
 315
 316         // Sanity check the bitmap
 317         LOG("Sanity checking plox");
 318         if( !CheckMem(Bitmap, sizeof(*Bitmap)) || !CheckMem(Bitmap->Data, Bitmap->W*Bitmap->H*sizeof(Uint32)) )
 319         {
 320                 Log_Warning("DrvUtil", "DrvUtil_Video_SetCursor: Bitmap (%p) is in invalid memory", Bitmap);
 321                 errno = -EINVAL;
 322                 LEAVE('i', -1);
 323                 return -1;
 324         }
 325
 326         // Don't take a copy of the DrvUtil provided cursor
 327         if( Bitmap == &gDrvUtil_TextModeCursor )
 328         {
 329                 LOG("No copy (provided cursor)");
 330                 Buf->CursorBitmap = Bitmap;
 331         }
 332         else
 333         {
 334                 LOG("Make copy");
 335                 size = sizeof(tVideo_IOCtl_Bitmap) + Bitmap->W*Bitmap->H*4;
 336
 337                 // Take a copy
 338                 Buf->CursorBitmap = malloc( size );
 339                 memcpy(Buf->CursorBitmap, Bitmap, size);
 340         }
 341
 342         // Restore cursor position
 343         LOG("Drawing");
 344         DrvUtil_Video_DrawCursor(Buf, csrX, csrY);
 345         LEAVE('i', 0);
 346         return 0;
 347 }
 348
 349 void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y)
 350 {
 351          int    render_ox=0, render_oy=0, render_w, render_h;
 352
 353         ENTER("pBuf iX iY", Buf, X, Y);
 354         DrvUtil_Video_RemoveCursor(Buf);
 355
 356         // X < 0 disables the cursor
 357         if( X < 0 ) {
 358                 Buf->CursorX = -1;
 359                 LEAVE('-');
 360                 return ;
 361         }
 362
 363         // Sanity checking
 364         if( X < 0 || Y < 0 || X >= Buf->Width || Y >= Buf->Height ) {
 365                 LEAVE('-');
 366                 return ;
 367         }
 368
 369         // Ensure the cursor is enabled
 370         if( !Buf->CursorBitmap ) {
 371                 LEAVE('-');
 372                 return ;
 373         }
 374
 375         // Save cursor position (for changing the bitmap)
 376         Buf->CursorX = X;       Buf->CursorY = Y;
 377         // Apply cursor's center offset
 378         X -= Buf->CursorBitmap->XOfs;
 379         Y -= Buf->CursorBitmap->YOfs;
 380
 381         // Get the width of the cursor on screen (clipping to right/bottom edges)
 382         render_w = X > Buf->Width  - Buf->CursorBitmap->W ? Buf->Width  - X : Buf->CursorBitmap->W;
 383         render_h = Y > Buf->Height - Buf->CursorBitmap->H ? Buf->Height - Y : Buf->CursorBitmap->H;
 384
 385         // Clipp to left/top edges
 386         if(X < 0) {     render_ox = -X; X = 0;  }
 387         if(Y < 0) {     render_oy = -Y; Y = 0;  }
 388
 389         // Save values
 390         Buf->CursorRenderW = render_w;  Buf->CursorRenderH = render_h;
 391         Buf->CursorDestX   = X;         Buf->CursorDestY = Y;
 392         Buf->CursorReadX   = render_ox; Buf->CursorReadY = render_oy;
 393
 394         LOG("%ix%i at %i,%i offset %i,%i",
 395                 render_w, render_h, X, Y, render_ox, render_oy);
 396
 397         // Call render routine
 398         DrvUtil_Video_RenderCursor(Buf);
 399         LEAVE('-');
 400 }
 401
 402 void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf)
 403 {
 404          int    src_x = Buf->CursorReadX, src_y = Buf->CursorReadY;
 405          int    render_w = Buf->CursorRenderW, render_h = Buf->CursorRenderH;
 406          int    dest_x = Buf->CursorDestX, dest_y = Buf->CursorDestY;
 407          int    bytes_per_px = (Buf->Depth + 7) / 8;
 408          int    save_pitch = Buf->CursorBitmap->W * bytes_per_px;
 409         void    *dest;
 410         Uint32  *src;
 411          int    x, y;
 412
 413         dest = (Uint8*)Buf->Framebuffer + dest_y*Buf->Pitch + dest_x*bytes_per_px;
 414         src = Buf->CursorBitmap->Data + src_y * Buf->CursorBitmap->W + src_x;
 415
 416         LOG("dest = %p, src = %p", dest, src);
 417
 418         // Allocate save buffer if not already
 419         if( !Buf->CursorSaveBuf )
 420                 Buf->CursorSaveBuf = malloc( Buf->CursorBitmap->W*Buf->CursorBitmap->H*bytes_per_px );
 421
 422         LOG("Saving back");
 423         // Save behind the cursor
 424         for( y = 0; y < render_h; y ++ )
 425                 memcpy(
 426                         (Uint8*)Buf->CursorSaveBuf + y*save_pitch,
 427                         (Uint8*)dest + y*Buf->Pitch,
 428                         render_w*bytes_per_px
 429                         );
 430
 431         // Draw the cursor
 432         switch(Buf->Depth)
 433         {
 434         case 15:
 435         case 16:
 436                 Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in cursor draw");
 437                 break;
 438         case 24:
 439                 LOG("24-bit render");
 440                 for( y = 0; y < render_h; y ++ )
 441                 {
 442                         Uint8   *px;
 443                         px = dest;
 444                         for(x = 0; x < render_w; x ++, px += 3)
 445                         {
 446                                 Uint32  value = src[x];
 447                                 // TODO: Should I implement alpha blending?
 448                                 if(value & 0xFF000000)
 449                                 {
 450                                         px[0] = value & 0xFF;
 451                                         px[1] = (value >> 8) & 0xFF;
 452                                         px[2] = (value >> 16) & 0xFF;
 453                                 }
 454                                 else
 455                                         ;
 456                         }
 457                         src += Buf->CursorBitmap->W;
 458                         dest = (Uint8*)dest + Buf->Pitch;
 459                 }
 460                 break;
 461         case 32:
 462                 LOG("32-bit render");
 463                 for( y = 0; y < render_h; y ++ )
 464                 {
 465                         Uint32  *px;
 466                         px = dest;
 467                         for(x = 0; x < render_w; x ++, px ++)
 468                         {
 469                                 Uint32  value = src[x];
 470                                 // TODO: Should I implement alpha blending?
 471                                 if(value & 0xFF000000)
 472                                         *px = value;
 473                                 else
 474                                         ;       // NOP, completely transparent
 475                         }
 476                         LOG("row %i/%i (%p-%P) done", y+1, render_h, dest, MM_GetPhysAddr(dest));
 477                         src += Buf->CursorBitmap->W;
 478                         dest = (Uint8*)dest + Buf->Pitch;
 479                 }
 480                 break;
 481         default:
 482                 Log_Error("DrvUtil", "RenderCursor - Unknown bit depth %i", Buf->Depth);
 483                 Buf->CursorX = -1;
 484                 break;
 485         }
 486 }
 487
 488 void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf)
 489 {
 490          int    bytes_per_px = (Buf->Depth + 7) / 8;
 491          int    y, save_pitch;
 492         Uint8   *dest, *src;
 493
 494         // Just a little sanity
 495         if( !Buf->CursorBitmap || Buf->CursorX == -1 )  return ;
 496         if( !Buf->CursorSaveBuf )       return ;
 497
 498 //      Debug("DrvUtil_Video_RemoveCursor: (Buf=%p) dest_x=%i, dest_y=%i", Buf, Buf->CursorDestX, Buf->CursorDestY);
 499
 500         // Set up
 501         save_pitch = Buf->CursorBitmap->W * bytes_per_px;
 502         dest = (Uint8*)Buf->Framebuffer + Buf->CursorDestY * Buf->Pitch + Buf->CursorDestX*bytes_per_px;
 503         src = Buf->CursorSaveBuf;
 504
 505         // Copy each line back
 506         for( y = 0; y < Buf->CursorRenderH; y ++ )
 507         {
 508                 memcpy( dest, src, Buf->CursorRenderW * bytes_per_px );
 509                 src += save_pitch;
 510                 dest += Buf->Pitch;
 511         }
 512
 513         // Set the cursor as removed
 514         Buf->CursorX = -1;
 515 }
 516
 517 void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour)
 518 {
 519         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 520
 521         // TODO: Handle non-32bit modes
 522         if( FBInfo->Depth != 32 )       return;
 523
 524         // TODO: Be less hacky
 525          int    pitch = FBInfo->Pitch/4;
 526         Uint32  *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X;
 527         while( H -- ) {
 528                 Uint32 *tmp;
 529                  int    i;
 530                 tmp = buf;
 531                 for(i=W;i--;tmp++)      *tmp = Colour;
 532                 buf += pitch;
 533         }
 534 }
 535
 536 void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H)
 537 {
 538         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 539          int    scrnpitch = FBInfo->Pitch;
 540          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 541          int    dst = DstY*scrnpitch + DstX;
 542          int    src = SrcY*scrnpitch + SrcX;
 543          int    tmp;
 544
 545         //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)",
 546         //      Ent, DstX, DstY, SrcX, SrcY, W, H);
 547
 548         if(SrcX + W > FBInfo->Width)    W = FBInfo->Width - SrcX;
 549         if(DstX + W > FBInfo->Width)    W = FBInfo->Width - DstX;
 550         if(SrcY + H > FBInfo->Height)   H = FBInfo->Height - SrcY;
 551         if(DstY + H > FBInfo->Height)   H = FBInfo->Height - DstY;
 552
 553         //Debug("W = %i, H = %i", W, H);
 554
 555         if( dst > src ) {
 556                 // Reverse copy
 557                 dst += H*scrnpitch;
 558                 src += H*scrnpitch;
 559                 while( H -- ) {
 560                         dst -= scrnpitch;
 561                         src -= scrnpitch;
 562                         tmp = W*bytes_per_px;
 563                         for( tmp = W; tmp --; ) {
 564                                 *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp);
 565                         }
 566                 }
 567         }
 568         else if(W == FBInfo->Width && FBInfo->Pitch == FBInfo->Width*bytes_per_px) {
 569                 memmove((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, H*FBInfo->Pitch);
 570         }
 571         else {
 572                 // Normal copy is OK
 573                 while( H -- ) {
 574                         memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px);
 575                         dst += scrnpitch;
 576                         src += scrnpitch;
 577                 }
 578         }
 579         //Log("Vesa_2D_Blit: RETURN");
 580 }
 581
 582
 583 // --- Disk Driver Helpers ---
 584 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
 585         tDrvUtil_Read_Callback ReadBlocks, Uint64 BlockSize, void *Argument)
 586 {
 587         Uint8   tmp[BlockSize]; // C99
 588         Uint64  block = Start / BlockSize;
 589          int    offset = Start - block * BlockSize;
 590          int    leading = BlockSize - offset;
 591         Uint64  num;
 592          int    tailings;
 593         Uint64  ret;
 594
 595         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize pArgument",
 596                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
 597
 598         // Non aligned start, let's fix that!
 599         if(offset != 0)
 600         {
 601                 if(leading > Length)    leading = Length;
 602                 LOG("Reading %i bytes from Block1+%i", leading, offset);
 603                 ret = ReadBlocks(block, 1, tmp, Argument);
 604                 if(ret != 1) {
 605                         LEAVE('i', 0);
 606                         return 0;
 607                 }
 608                 memcpy( Buffer, &tmp[offset], leading );
 609
 610                 if(leading == Length) {
 611                         LEAVE('i', leading);
 612                         return leading;
 613                 }
 614
 615                 Buffer = (Uint8*)Buffer + leading;
 616                 block ++;
 617                 num = ( Length - leading ) / BlockSize;
 618                 tailings = Length - num * BlockSize - leading;
 619         }
 620         else {
 621                 num = Length / BlockSize;
 622                 tailings = Length % BlockSize;
 623         }
 624
 625         // Read central blocks
 626         if(num)
 627         {
 628                 LOG("Reading %i blocks", num);
 629                 ret = ReadBlocks(block, num, Buffer, Argument);
 630                 if(ret != num ) {
 631                         LEAVE('X', leading + ret * BlockSize);
 632                         return leading + ret * BlockSize;
 633                 }
 634         }
 635
 636         // Read last tailing block
 637         if(tailings != 0)
 638         {
 639                 LOG("Reading %i bytes from last block", tailings);
 640                 block += num;
 641                 Buffer = (Uint8*)Buffer + num * BlockSize;
 642                 ret = ReadBlocks(block, 1, tmp, Argument);
 643                 if(ret != 1) {
 644                         LEAVE('X', leading + num * BlockSize);
 645                         return leading + num * BlockSize;
 646                 }
 647                 memcpy( Buffer, tmp, tailings );
 648         }
 649
 650         LEAVE('X', Length);
 651         return Length;
 652 }
 653
 654 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, const void *Buffer,
 655         tDrvUtil_Read_Callback ReadBlocks, tDrvUtil_Write_Callback WriteBlocks,
 656         Uint64 BlockSize, void *Argument)
 657 {
 658         Uint8   tmp[BlockSize]; // C99
 659         Uint64  block = Start / BlockSize;
 660          int    offset = Start - block * BlockSize;
 661          int    leading = BlockSize - offset;
 662         Uint64  num;
 663          int    tailings;
 664         Uint64  ret;
 665
 666         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize pArgument",
 667                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
 668
 669         // Non aligned start, let's fix that!
 670         if(offset != 0)
 671         {
 672                 if(leading > Length)    leading = Length;
 673                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 674                 // Read a copy of the block
 675                 ret = ReadBlocks(block, 1, tmp, Argument);
 676                 if(ret != 1) {
 677                         LEAVE('i', 0);
 678                         return 0;
 679                 }
 680                 // Modify
 681                 memcpy( &tmp[offset], Buffer, leading );
 682                 // Write Back
 683                 ret = WriteBlocks(block, 1, tmp, Argument);
 684                 if(ret != 1) {
 685                         LEAVE('i', 0);
 686                         return 0;
 687                 }
 688
 689                 if(leading == Length) {
 690                         LEAVE('i', leading);
 691                         return leading;
 692                 }
 693
 694                 Buffer = (Uint8*)Buffer + leading;
 695                 block ++;
 696                 num = ( Length - leading ) / BlockSize;
 697                 tailings = Length - num * BlockSize - leading;
 698         }
 699         else {
 700                 num = Length / BlockSize;
 701                 tailings = Length % BlockSize;
 702         }
 703
 704         // Read central blocks
 705         if(num)
 706         {
 707                 LOG("Writing %i blocks", num);
 708                 ret = WriteBlocks(block, num, Buffer, Argument);
 709                 if(ret != num ) {
 710                         LEAVE('X', leading + ret * BlockSize);
 711                         return leading + ret * BlockSize;
 712                 }
 713         }
 714
 715         // Read last tailing block
 716         if(tailings != 0)
 717         {
 718                 LOG("Writing %i bytes to last block", tailings);
 719                 block += num;
 720                 Buffer = (Uint8*)Buffer + num * BlockSize;
 721                 // Read
 722                 ret = ReadBlocks(block, 1, tmp, Argument);
 723                 if(ret != 1) {
 724                         LEAVE('X', leading + num * BlockSize);
 725                         return leading + num * BlockSize;
 726                 }
 727                 // Modify
 728                 memcpy( tmp, Buffer, tailings );
 729                 // Write
 730                 ret = WriteBlocks(block, 1, tmp, Argument);
 731                 if(ret != 1) {
 732                         LEAVE('X', leading + num * BlockSize);
 733                         return leading + num * BlockSize;
 734                 }
 735
 736         }
 737
 738         LEAVE('X', Length);
 739         return Length;
 740 }