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                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  37                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  38                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  39                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  40                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  41                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  42                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  43                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  44                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  45                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  46                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  47                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  48                 -1, 0xFF000000, 0, 0, 0, 0, 0, 0,
  49                 -1, 0xFF000000, 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, void *Buffer, int Length,
  57         tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers)
  58 {
  59         void    *stream = Buffer;
  60          int    rem = Length;
  61          int    op;
  62         while( rem )
  63         {
  64                 rem --;
  65                 op = *(Uint8*)stream;
  66                 stream = (void*)((tVAddr)stream + 1);
  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 < 12)    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                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
  98                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
  99                                 ((Uint32*)stream)[2]
 100                                 );
 101
 102                         rem -= 12;
 103                         stream = (void*)((tVAddr)stream + 12);
 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                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
 118                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
 119                                 ((Uint16*)stream)[4], ((Uint16*)stream)[5]
 120                                 );
 121
 122                         rem -= 12;
 123                         stream = (void*)((tVAddr)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, void *Buffer)
 132 {
 133         Uint8   *dest;
 134         ENTER("pFBInfo xOffset xLength pBuffer",
 135                 Mode, FBInfo, Offset, Length, Buffer);
 136         switch( FBInfo->BufferFormat )
 137         {
 138         case VIDEO_BUFFMT_TEXT:
 139                 {
 140                 tVT_Char        *chars = Buffer;
 141                  int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 142                  int    widthInChars = FBInfo->Width/giVT_CharWidth;
 143                  int    heightInChars = FBInfo->Height/giVT_CharHeight;
 144                  int    x, y, i;
 145
 146                 LOG("bytes_per_px = %i", bytes_per_px);
 147                 LOG("widthInChars = %i, heightInChars = %i", widthInChars, heightInChars);
 148
 149                 Length /= sizeof(tVT_Char);     Offset /= sizeof(tVT_Char);
 150
 151                 x = Offset % widthInChars;      y = Offset / widthInChars;
 152                 LOG("x = %i, y = %i", x, y);
 153
 154                 // Sanity Check
 155                 if(Offset > heightInChars * widthInChars)       LEAVE_RET('i', 0);
 156                 if(y >= heightInChars)  LEAVE_RET('i', 0);
 157
 158                 if( Offset + Length > heightInChars*widthInChars )
 159                 {
 160                         Length = heightInChars*widthInChars - Offset;
 161                 }
 162
 163                 dest = FBInfo->Framebuffer;
 164                 LOG("dest = %p", dest);
 165                 dest += y * giVT_CharHeight * FBInfo->Pitch;
 166                 LOG("dest = %p", dest);
 167
 168                 for( i = 0; i < Length; i++ )
 169                 {
 170                         if( y >= heightInChars )
 171                         {
 172                                 Log_Notice("DrvUtil", "Stopped at %i", i);
 173                                 break;
 174                         }
 175
 176                         VT_Font_Render(
 177                                 chars->Ch,
 178                                 dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch,
 179                                 VT_Colour12toN(chars->BGCol, FBInfo->Depth),
 180                                 VT_Colour12toN(chars->FGCol, FBInfo->Depth)
 181                                 );
 182
 183                         chars ++;
 184                         x ++;
 185                         if( x >= widthInChars )
 186                         {
 187                                 x = 0;
 188                                 y ++;
 189                                 dest += FBInfo->Pitch*giVT_CharHeight;
 190                                 LOG("dest = %p", dest);
 191                         }
 192                 }
 193                 Length = i * sizeof(tVT_Char);
 194                 }
 195                 break;
 196
 197         case VIDEO_BUFFMT_FRAMEBUFFER:
 198                 if(FBInfo->Width*FBInfo->Height*4 < Offset+Length)
 199                 {
 200                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow");
 201                         return 0;
 202                 }
 203
 204                 //TODO: Handle non 32-bpp framebuffer modes
 205                 if( FBInfo->Depth != 32 ) {
 206                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Don't support non 32-bpp FB mode");
 207                         return 0;
 208                 }
 209
 210
 211                 //TODO: Handle pitch != Width*BytesPerPixel
 212                 // Copy to Frambuffer
 213                 dest = (Uint8 *)FBInfo->Framebuffer + Offset;
 214                 memcpy(dest, Buffer, Length);
 215                 break;
 216
 217         case VIDEO_BUFFMT_2DSTREAM:
 218                 Length = DrvUtil_Video_2DStream(
 219                         FBInfo, Buffer, Length,
 220                         &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions)
 221                         );
 222                 break;
 223
 224         default:
 225                 LEAVE('i', -1);
 226                 return -1;
 227         }
 228         LEAVE('x', Length);
 229         return Length;
 230 }
 231
 232 void DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap)
 233 {
 234          int    csrX = Buf->CursorX, csrY = Buf->CursorY;
 235         size_t  size;
 236
 237         // Clear old bitmap
 238         if( Buf->CursorBitmap )
 239         {
 240                 DrvUtil_Video_RemoveCursor(Buf);
 241                 if( !Bitmap || Bitmap->W != Buf->CursorBitmap->W || Bitmap->H != Buf->CursorBitmap->H )
 242                 {
 243                         free( Buf->CursorSaveBuf );
 244                         Buf->CursorSaveBuf = NULL;
 245                 }
 246                 if( Buf->CursorBitmap != &gDrvUtil_TextModeCursor)
 247                         free(Buf->CursorBitmap);
 248                 Buf->CursorBitmap = NULL;
 249         }
 250
 251         // If the new bitmap is null, disable drawing
 252         if( !Bitmap )
 253         {
 254                 Buf->CursorX = -1;
 255                 Buf->CursorY = -1;
 256                 return ;
 257         }
 258
 259         if( Bitmap != &gDrvUtil_TextModeCursor )
 260         {
 261                 // Check the new bitmap is valid
 262                 size = sizeof(tVideo_IOCtl_Bitmap) + Bitmap->W*Bitmap->H*4;
 263                 if( !CheckMem(Bitmap, size) ) {
 264                         Log_Warning("DrvUtil", "DrvUtil_Video_SetCursor: Bitmap (%p) is in invalid memory", Bitmap);
 265                         return;
 266                 }
 267
 268                 // Take a copy
 269                 Buf->CursorBitmap = malloc( size );
 270                 memcpy(Buf->CursorBitmap, Bitmap, size);
 271         }
 272         else
 273         {
 274                 Buf->CursorBitmap = &gDrvUtil_TextModeCursor;
 275         }
 276
 277         // Restore cursor position
 278         DrvUtil_Video_DrawCursor(Buf, csrX, csrY);
 279 }
 280
 281 void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y)
 282 {
 283          int    render_ox=0, render_oy=0, render_w, render_h;
 284
 285         DrvUtil_Video_RemoveCursor(Buf);
 286
 287         // X < 0 disables the cursor
 288         if( X < 0 ) {
 289                 Buf->CursorX = -1;
 290                 return ;
 291         }
 292
 293         // Sanity checking
 294         if( X < 0 || Y < 0 )    return;
 295         if( X >= Buf->Width || Y >= Buf->Height )       return;
 296
 297         // Ensure the cursor is enabled
 298         if( !Buf->CursorBitmap )        return ;
 299
 300         // Save cursor position (for changing the bitmap)
 301         Buf->CursorX = X;       Buf->CursorY = Y;
 302
 303         // Apply cursor's center offset
 304         X -= Buf->CursorBitmap->XOfs;
 305         Y -= Buf->CursorBitmap->YOfs;
 306
 307         // Get the width of the cursor on screen (clipping to right/bottom edges)
 308         render_w = X > Buf->Width  - Buf->CursorBitmap->W ? Buf->Width  - X : Buf->CursorBitmap->W;
 309         render_h = Y > Buf->Height - Buf->CursorBitmap->H ? Buf->Height - Y : Buf->CursorBitmap->H;
 310
 311         // Clipp to left/top edges
 312         if(X < 0) {     render_ox = -X; X = 0;  }
 313         if(Y < 0) {     render_oy = -Y; Y = 0;  }
 314
 315         // Save values
 316         Buf->CursorRenderW = render_w;  Buf->CursorRenderH = render_h;
 317         Buf->CursorDestX   = X;         Buf->CursorDestY = Y;
 318         Buf->CursorReadX   = render_ox; Buf->CursorReadY = render_oy;
 319
 320         // Call render routine
 321         DrvUtil_Video_RenderCursor(Buf);
 322 }
 323
 324 void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf)
 325 {
 326          int    src_x = Buf->CursorReadX, src_y = Buf->CursorReadY;
 327          int    render_w = Buf->CursorRenderW, render_h = Buf->CursorRenderH;
 328          int    dest_x = Buf->CursorDestX, dest_y = Buf->CursorDestY;
 329          int    bytes_per_px = (Buf->Depth + 7) / 8;
 330          int    save_pitch = Buf->CursorBitmap->W * bytes_per_px;
 331         void    *dest;
 332         Uint32  *src;
 333          int    x, y;
 334
 335 //      Debug("DrvUtil_Video_RenderCursor: (Buf=%p) dest_x=%i, dest_y=%i", Buf, dest_x, dest_y);
 336
 337         dest = (Uint8*)Buf->Framebuffer + dest_y*Buf->Pitch + dest_x*bytes_per_px;
 338         src = Buf->CursorBitmap->Data + src_y * Buf->CursorBitmap->W + src_x;
 339
 340         // Allocate save buffer if not already
 341         if( !Buf->CursorSaveBuf )
 342                 Buf->CursorSaveBuf = malloc( Buf->CursorBitmap->W*Buf->CursorBitmap->H*bytes_per_px );
 343
 344         // Save behind the cursor
 345         for( y = 0; y < render_h; y ++ )
 346                 memcpy(
 347                         (Uint8*)Buf->CursorSaveBuf + y*save_pitch,
 348                         (Uint8*)dest + y*Buf->Pitch,
 349                         render_w*bytes_per_px
 350                         );
 351         // Draw the cursor
 352         switch(Buf->Depth)
 353         {
 354         case 32:
 355                 for( y = 0; y < render_h; y ++ )
 356                 {
 357                         for(x = 0; x < render_w; x ++ )
 358                         {
 359                                 // TODO: Should I implement alpha blending?
 360                                 if(src[x] & 0xFF000000)
 361                                         ((Uint32*)dest)[x] = src[x];
 362                                 else
 363                                         ;       // NOP, completely transparent
 364                         }
 365                         src += Buf->CursorBitmap->W;
 366                         dest = (Uint8*)dest + Buf->Pitch;
 367                 }
 368                 break;
 369         default:
 370                 Log_Error("DrvUtil", "TODO: Implement non 32-bpp cursor");
 371                 break;
 372         }
 373 }
 374
 375 void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf)
 376 {
 377          int    bytes_per_px = (Buf->Depth + 7) / 8;
 378          int    y, save_pitch;
 379         Uint8   *dest, *src;
 380
 381         // Just a little sanity
 382         if( !Buf->CursorBitmap || Buf->CursorX == -1 )  return ;
 383         if( !Buf->CursorSaveBuf )       return ;
 384
 385 //      Debug("DrvUtil_Video_RemoveCursor: (Buf=%p) dest_x=%i, dest_y=%i", Buf, Buf->CursorDestX, Buf->CursorDestY);
 386
 387         // Set up
 388         save_pitch = Buf->CursorBitmap->W * bytes_per_px;
 389         dest = (Uint8*)Buf->Framebuffer + Buf->CursorDestY * Buf->Pitch + Buf->CursorDestX*bytes_per_px;
 390         src = Buf->CursorSaveBuf;
 391
 392         // Copy each line back
 393         for( y = 0; y < Buf->CursorRenderH; y ++ )
 394         {
 395                 memcpy( dest, src, Buf->CursorRenderW * bytes_per_px );
 396                 src += save_pitch;
 397                 dest += Buf->Pitch;
 398         }
 399
 400         // Set the cursor as removed
 401         Buf->CursorX = -1;
 402 }
 403
 404 void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour)
 405 {
 406         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 407
 408         // TODO: Handle non-32bit modes
 409         if( FBInfo->Depth != 32 )       return;
 410
 411         // TODO: Be less hacky
 412          int    pitch = FBInfo->Pitch/4;
 413         Uint32  *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X;
 414         while( H -- ) {
 415                 Uint32 *tmp;
 416                  int    i;
 417                 tmp = buf;
 418                 for(i=W;i--;tmp++)      *tmp = Colour;
 419                 buf += pitch;
 420         }
 421 }
 422
 423 void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H)
 424 {
 425         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 426          int    scrnpitch = FBInfo->Pitch;
 427          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 428          int    dst = DstY*scrnpitch + DstX;
 429          int    src = SrcY*scrnpitch + SrcX;
 430          int    tmp;
 431
 432         //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)",
 433         //      Ent, DstX, DstY, SrcX, SrcY, W, H);
 434
 435         if(SrcX + W > FBInfo->Width)    W = FBInfo->Width - SrcX;
 436         if(DstX + W > FBInfo->Width)    W = FBInfo->Width - DstX;
 437         if(SrcY + H > FBInfo->Height)   H = FBInfo->Height - SrcY;
 438         if(DstY + H > FBInfo->Height)   H = FBInfo->Height - DstY;
 439
 440         //Debug("W = %i, H = %i", W, H);
 441
 442         if( dst > src ) {
 443                 // Reverse copy
 444                 dst += H*scrnpitch;
 445                 src += H*scrnpitch;
 446                 while( H -- ) {
 447                         dst -= scrnpitch;
 448                         src -= scrnpitch;
 449                         tmp = W*bytes_per_px;
 450                         for( tmp = W; tmp --; ) {
 451                                 *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp);
 452                         }
 453                 }
 454         }
 455         else {
 456                 // Normal copy is OK
 457                 while( H -- ) {
 458                         memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px);
 459                         dst += scrnpitch;
 460                         src += scrnpitch;
 461                 }
 462         }
 463         //Log("Vesa_2D_Blit: RETURN");
 464 }
 465
 466
 467 // --- Disk Driver Helpers ---
 468 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
 469         tDrvUtil_Callback ReadBlocks, Uint64 BlockSize, Uint Argument)
 470 {
 471         Uint8   tmp[BlockSize]; // C99
 472         Uint64  block = Start / BlockSize;
 473          int    offset = Start - block * BlockSize;
 474          int    leading = BlockSize - offset;
 475         Uint64  num;
 476          int    tailings;
 477         Uint64  ret;
 478
 479         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize xArgument",
 480                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
 481
 482         // Non aligned start, let's fix that!
 483         if(offset != 0)
 484         {
 485                 if(leading > Length)    leading = Length;
 486                 LOG("Reading %i bytes from Block1+%i", leading, offset);
 487                 ret = ReadBlocks(block, 1, tmp, Argument);
 488                 if(ret != 1) {
 489                         LEAVE('i', 0);
 490                         return 0;
 491                 }
 492                 memcpy( Buffer, &tmp[offset], leading );
 493
 494                 if(leading == Length) {
 495                         LEAVE('i', leading);
 496                         return leading;
 497                 }
 498
 499                 Buffer = (Uint8*)Buffer + leading;
 500                 block ++;
 501                 num = ( Length - leading ) / BlockSize;
 502                 tailings = Length - num * BlockSize - leading;
 503         }
 504         else {
 505                 num = Length / BlockSize;
 506                 tailings = Length % BlockSize;
 507         }
 508
 509         // Read central blocks
 510         if(num)
 511         {
 512                 LOG("Reading %i blocks", num);
 513                 ret = ReadBlocks(block, num, Buffer, Argument);
 514                 if(ret != num ) {
 515                         LEAVE('X', leading + ret * BlockSize);
 516                         return leading + ret * BlockSize;
 517                 }
 518         }
 519
 520         // Read last tailing block
 521         if(tailings != 0)
 522         {
 523                 LOG("Reading %i bytes from last block", tailings);
 524                 block += num;
 525                 Buffer = (Uint8*)Buffer + num * BlockSize;
 526                 ret = ReadBlocks(block, 1, tmp, Argument);
 527                 if(ret != 1) {
 528                         LEAVE('X', leading + num * BlockSize);
 529                         return leading + num * BlockSize;
 530                 }
 531                 memcpy( Buffer, tmp, tailings );
 532         }
 533
 534         LEAVE('X', Length);
 535         return Length;
 536 }
 537
 538 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, void *Buffer,
 539         tDrvUtil_Callback ReadBlocks, tDrvUtil_Callback WriteBlocks,
 540         Uint64 BlockSize, Uint Argument)
 541 {
 542         Uint8   tmp[BlockSize]; // C99
 543         Uint64  block = Start / BlockSize;
 544          int    offset = Start - block * BlockSize;
 545          int    leading = BlockSize - offset;
 546         Uint64  num;
 547          int    tailings;
 548         Uint64  ret;
 549
 550         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
 551                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
 552
 553         // Non aligned start, let's fix that!
 554         if(offset != 0)
 555         {
 556                 if(leading > Length)    leading = Length;
 557                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 558                 // Read a copy of the block
 559                 ret = ReadBlocks(block, 1, tmp, Argument);
 560                 if(ret != 1) {
 561                         LEAVE('i', 0);
 562                         return 0;
 563                 }
 564                 // Modify
 565                 memcpy( &tmp[offset], Buffer, leading );
 566                 // Write Back
 567                 ret = WriteBlocks(block, 1, tmp, Argument);
 568                 if(ret != 1) {
 569                         LEAVE('i', 0);
 570                         return 0;
 571                 }
 572
 573                 if(leading == Length) {
 574                         LEAVE('i', leading);
 575                         return leading;
 576                 }
 577
 578                 Buffer = (Uint8*)Buffer + leading;
 579                 block ++;
 580                 num = ( Length - leading ) / BlockSize;
 581                 tailings = Length - num * BlockSize - leading;
 582         }
 583         else {
 584                 num = Length / BlockSize;
 585                 tailings = Length % BlockSize;
 586         }
 587
 588         // Read central blocks
 589         if(num)
 590         {
 591                 LOG("Writing %i blocks", num);
 592                 ret = WriteBlocks(block, num, Buffer, Argument);
 593                 if(ret != num ) {
 594                         LEAVE('X', leading + ret * BlockSize);
 595                         return leading + ret * BlockSize;
 596                 }
 597         }
 598
 599         // Read last tailing block
 600         if(tailings != 0)
 601         {
 602                 LOG("Writing %i bytes to last block", tailings);
 603                 block += num;
 604                 Buffer = (Uint8*)Buffer + num * BlockSize;
 605                 // Read
 606                 ret = ReadBlocks(block, 1, tmp, Argument);
 607                 if(ret != 1) {
 608                         LEAVE('X', leading + num * BlockSize);
 609                         return leading + num * BlockSize;
 610                 }
 611                 // Modify
 612                 memcpy( tmp, Buffer, tailings );
 613                 // Write
 614                 ret = WriteBlocks(block, 1, tmp, Argument);
 615                 if(ret != 1) {
 616                         LEAVE('X', leading + num * BlockSize);
 617                         return leading + num * BlockSize;
 618                 }
 619
 620         }
 621
 622         LEAVE('X', Length);
 623         return Length;
 624 }