Kernel/drvutil.c

   1 /*
   2  * Acess2
   3  * Common Driver/Filesystem Helper Functions
   4  */
   5 #define DEBUG   0
   6 #include <acess.h>
   7 #include <api_drv_disk.h>
   8 #include <api_drv_video.h>
   9
  10 // === TYPES ===
  11
  12 // === PROTOTYPES ===
  13 //int   DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length, tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers);
  14 //size_t        DrvUtil_Video_WriteLFB(int Mode, tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, void *Src);
  15 void    DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour);
  16 void    DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H);
  17
  18 // === GLOBALS ===
  19 tDrvUtil_Video_2DHandlers       gDrvUtil_Stub_2DFunctions = {
  20         NULL,
  21         DrvUtil_Video_2D_Fill,
  22         DrvUtil_Video_2D_Blit
  23 };
  24
  25 // === CODE ===
  26 // --- Video Driver Helpers ---
  27 int DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length,
  28         tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers)
  29 {
  30         void    *stream = Buffer;
  31          int    rem = Length;
  32          int    op;
  33         while( rem )
  34         {
  35                 rem --;
  36                 op = *(Uint8*)stream;
  37                 stream = (void*)((tVAddr)stream + 1);
  38
  39                 if(op > NUM_VIDEO_2DOPS) {
  40                         Log_Warning("DrvUtil",
  41                                 "DrvUtil_Video_2DStream: Unknown operation %i",
  42                                 op);
  43                         return Length-rem;
  44                 }
  45
  46                 if(op*sizeof(void*) > SizeofHandlers) {
  47                         Log_Warning("DrvUtil",
  48                                 "DrvUtil_Video_2DStream: Driver does not support op %i",
  49                                 op);
  50                         return Length-rem;
  51                 }
  52
  53                 switch(op)
  54                 {
  55                 case VIDEO_2DOP_NOP:    break;
  56
  57                 case VIDEO_2DOP_FILL:
  58                         if(rem < 12)    return Length-rem;
  59
  60                         if(!Handlers->Fill) {
  61                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
  62                                         " does not support VIDEO_2DOP_FILL");
  63                                 return Length-rem;
  64                         }
  65
  66                         Handlers->Fill(
  67                                 Ent,
  68                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
  69                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
  70                                 ((Uint32*)stream)[2]
  71                                 );
  72
  73                         rem -= 12;
  74                         stream = (void*)((tVAddr)stream + 12);
  75                         break;
  76
  77                 case VIDEO_2DOP_BLIT:
  78                         if(rem < 12)    return Length-rem;
  79
  80                         if(!Handlers->Blit) {
  81                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
  82                                         " does not support VIDEO_2DOP_BLIT");
  83                                 return Length-rem;
  84                         }
  85
  86                         Handlers->Blit(
  87                                 Ent,
  88                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
  89                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
  90                                 ((Uint16*)stream)[4], ((Uint16*)stream)[5]
  91                                 );
  92
  93                         rem -= 12;
  94                         stream = (void*)((tVAddr)stream + 12);
  95                         break;
  96
  97                 }
  98         }
  99         return 0;
 100 }
 101
 102 int DrvUtil_Video_WriteLFB(int Mode, tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, void *Buffer)
 103 {
 104         Uint8   *dest;
 105         ENTER("iMode pFBInfo xOffset xLength pBuffer",
 106                 Mode, FBInfo, Offset, Length, Buffer);
 107         switch( Mode )
 108         {
 109         case VIDEO_BUFFMT_TEXT:
 110                 {
 111                 tVT_Char        *chars = Buffer;
 112                  int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 113                  int    widthInChars = FBInfo->Width/giVT_CharWidth;
 114                  int    heightInChars = FBInfo->Height/giVT_CharHeight;
 115                  int    x, y, i;
 116
 117                 LOG("bytes_per_px = %i", bytes_per_px);
 118                 LOG("widthInChars = %i, heightInChars = %i", widthInChars, heightInChars);
 119
 120                 Length /= sizeof(tVT_Char);     Offset /= sizeof(tVT_Char);
 121
 122                 x = Offset % widthInChars;      y = Offset / widthInChars;
 123                 LOG("x = %i, y = %i", x, y);
 124
 125                 // Sanity Check
 126                 if(Offset > heightInChars * widthInChars)       LEAVE_RET('i', 0);
 127                 if(y >= heightInChars)  LEAVE_RET('i', 0);
 128
 129                 if( Offset + Length > heightInChars*widthInChars )
 130                 {
 131                         Length = heightInChars*widthInChars - Offset;
 132                 }
 133
 134                 dest = FBInfo->Framebuffer;
 135                 LOG("dest = %p", dest);
 136                 dest += y * giVT_CharHeight * FBInfo->Pitch;
 137                 LOG("dest = %p", dest);
 138
 139                 for( i = 0; i < Length; i++ )
 140                 {
 141                         if( y >= heightInChars )
 142                         {
 143                                 Log_Notice("DrvUtil", "Stopped at %i", i);
 144                                 break;
 145                         }
 146
 147 //                      LOG("chars->Ch=0x%x,chars->BGCol=0x%x,chars->FGCol=0x%x",
 148 //                              chars->Ch, chars->BGCol, chars->FGCol);
 149
 150                         VT_Font_Render(
 151                                 chars->Ch,
 152                                 dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch,
 153                                 VT_Colour12toN(chars->BGCol, FBInfo->Depth),
 154                                 VT_Colour12toN(chars->FGCol, FBInfo->Depth)
 155                                 );
 156
 157                         chars ++;
 158                         x ++;
 159                         if( x >= widthInChars )
 160                         {
 161                                 x = 0;
 162                                 y ++;
 163                                 dest += FBInfo->Pitch*giVT_CharHeight;
 164                                 LOG("dest = %p", dest);
 165                         }
 166                 }
 167                 Length = i * sizeof(tVT_Char);
 168                 }
 169                 break;
 170
 171         case VIDEO_BUFFMT_FRAMEBUFFER:
 172                 if(FBInfo->Width*FBInfo->Height*4 < Offset+Length)
 173                 {
 174                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow");
 175                         return 0;
 176                 }
 177
 178                 //TODO: Handle non 32-bpp framebuffer modes
 179                 if( FBInfo->Depth != 32 ) {
 180                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Don't support non 32-bpp FB mode");
 181                         return 0;
 182                 }
 183
 184
 185                 //TODO: Handle pitch != Width*BytesPerPixel
 186                 // Copy to Frambuffer
 187                 dest = (Uint8 *)FBInfo->Framebuffer + Offset;
 188                 memcpy(dest, Buffer, Length);
 189                 break;
 190
 191         case VIDEO_BUFFMT_2DSTREAM:
 192                 Length = DrvUtil_Video_2DStream(
 193                         FBInfo, Buffer, Length,
 194                         &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions)
 195                         );
 196                 break;
 197
 198         default:
 199                 LEAVE('i', -1);
 200                 return -1;
 201         }
 202         LEAVE('x', Length);
 203         return Length;
 204 }
 205
 206 void DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap)
 207 {
 208          int    csrX = Buf->CursorX, csrY = Buf->CursorY;
 209         size_t  size;
 210
 211         // Clear old bitmap
 212         if( Buf->CursorBitmap )
 213         {
 214                 DrvUtil_Video_RemoveCursor(Buf);
 215                 if( Bitmap->W != Buf->CursorBitmap->W || Bitmap->H != Buf->CursorBitmap->H )
 216                 {
 217                         free( Buf->CursorSaveBuf );
 218                         Buf->CursorSaveBuf = NULL;
 219                 }
 220                 free(Buf->CursorBitmap);
 221                 Buf->CursorBitmap = NULL;
 222         }
 223
 224         // Check the new bitmap is valid
 225         size = sizeof(tVideo_IOCtl_Bitmap) + Bitmap->W*Bitmap->H*4;
 226         if( !CheckMem(Bitmap, size) ) {
 227                 Log_Warning("DrvUtil", "DrvUtil_Video_SetCursor: Bitmap (%p) invalid mem", Bitmap);
 228                 return;
 229         }
 230
 231         // Take a copy
 232         Buf->CursorBitmap = malloc( size );
 233         memcpy(Buf->CursorBitmap, Bitmap, size);
 234
 235         // Restore cursor position
 236         DrvUtil_Video_DrawCursor(Buf, csrX, csrY);
 237 }
 238
 239 void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y)
 240 {
 241          int    bytes_per_px = (Buf->Depth + 7) / 8;
 242         Uint32  *dest, *src;
 243          int    save_pitch, y;
 244          int    render_ox=0, render_oy=0, render_w, render_h;
 245
 246         if( X < 0 || Y < 0 )    return;
 247         if( X >= Buf->Width || Y >= Buf->Height )       return;
 248
 249         if( !Buf->CursorBitmap )        return ;
 250
 251         X -= Buf->CursorBitmap->XOfs;
 252         Y -= Buf->CursorBitmap->YOfs;
 253
 254         render_w = X > Buf->Width  - Buf->CursorBitmap->W ? Buf->Width  - X : Buf->CursorBitmap->W;
 255         render_h = Y > Buf->Height - Buf->CursorBitmap->H ? Buf->Height - Y : Buf->CursorBitmap->H;
 256
 257         if(X < 0) {
 258                 render_ox = -X;
 259                 X = 0;
 260         }
 261         if(Y < 0) {
 262                 render_oy = -Y;
 263                 Y = 0;
 264         }
 265
 266         save_pitch = Buf->CursorBitmap->W * bytes_per_px;
 267
 268         dest = (void*)( (tVAddr)Buf->Framebuffer + Y * Buf->Pitch + X*bytes_per_px );
 269         src = Buf->CursorBitmap->Data;
 270         if( !Buf->CursorSaveBuf )
 271                 Buf->CursorSaveBuf = malloc( Buf->CursorBitmap->W*Buf->CursorBitmap->H*bytes_per_px );
 272         // Save behind the cursor
 273         for( y = render_oy; y < render_h; y ++ )
 274                 memcpy(
 275                         (Uint8*)Buf->CursorSaveBuf + y*save_pitch,
 276                         (Uint8*)dest + render_ox*bytes_per_px + y*Buf->Pitch,
 277                         render_w*bytes_per_px
 278                         );
 279         // Draw the cursor
 280         switch(Buf->Depth)
 281         {
 282         case 32:
 283                 src += render_oy * Buf->CursorBitmap->W;
 284                 for( y = render_oy; y < render_h; y ++ )
 285                 {
 286                         int x;
 287                         for(x = render_ox; x < render_w; x ++ )
 288                         {
 289                                 if(src[x] & 0xFF000000)
 290                                         dest[x] = src[x];
 291                                 else
 292                                         ;       // NOP, completely transparent
 293                         }
 294                         src += Buf->CursorBitmap->W;
 295                         dest = (void*)((Uint8*)dest + Buf->Pitch);
 296                 }
 297                 break;
 298         default:
 299                 Log_Error("DrvUtil", "TODO: Implement non 32-bpp cursor");
 300                 break;
 301         }
 302         Buf->CursorX = X;
 303         Buf->CursorY = Y;
 304 }
 305
 306 void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf)
 307 {
 308          int    bytes_per_px = (Buf->Depth + 7) / 8;
 309          int    y, save_pitch;
 310         Uint32  *dest;
 311
 312         if( !Buf->CursorBitmap || Buf->CursorX == -1 )  return ;
 313
 314         if( !Buf->CursorSaveBuf )
 315                 return ;
 316
 317         save_pitch = Buf->CursorBitmap->W * bytes_per_px;
 318         dest = (void*)( (tVAddr)Buf->Framebuffer + Buf->CursorY * Buf->Pitch + Buf->CursorX*bytes_per_px );
 319         for( y = 0; y < Buf->CursorBitmap->H; y ++ )
 320                 memcpy( (Uint8*)dest + y*Buf->Pitch, (Uint8*)Buf->CursorSaveBuf + y*save_pitch, save_pitch);
 321
 322         Buf->CursorX = -1;
 323 }
 324
 325 void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour)
 326 {
 327         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 328
 329         // TODO: Handle non-32bit modes
 330         if( FBInfo->Depth != 32 )       return;
 331
 332         // TODO: Be less hacky
 333          int    pitch = FBInfo->Pitch/4;
 334         Uint32  *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X;
 335         while( H -- ) {
 336                 Uint32 *tmp;
 337                  int    i;
 338                 tmp = buf;
 339                 for(i=W;i--;tmp++)      *tmp = Colour;
 340                 buf += pitch;
 341         }
 342 }
 343
 344 void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H)
 345 {
 346         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 347          int    scrnpitch = FBInfo->Pitch;
 348          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 349          int    dst = DstY*scrnpitch + DstX;
 350          int    src = SrcY*scrnpitch + SrcX;
 351          int    tmp;
 352
 353         //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)",
 354         //      Ent, DstX, DstY, SrcX, SrcY, W, H);
 355
 356         if(SrcX + W > FBInfo->Width)    W = FBInfo->Width - SrcX;
 357         if(DstX + W > FBInfo->Width)    W = FBInfo->Width - DstX;
 358         if(SrcY + H > FBInfo->Height)   H = FBInfo->Height - SrcY;
 359         if(DstY + H > FBInfo->Height)   H = FBInfo->Height - DstY;
 360
 361         //Debug("W = %i, H = %i", W, H);
 362
 363         if( dst > src ) {
 364                 // Reverse copy
 365                 dst += H*scrnpitch;
 366                 src += H*scrnpitch;
 367                 while( H -- ) {
 368                         dst -= scrnpitch;
 369                         src -= scrnpitch;
 370                         tmp = W*bytes_per_px;
 371                         for( tmp = W; tmp --; ) {
 372                                 *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp);
 373                         }
 374                 }
 375         }
 376         else {
 377                 // Normal copy is OK
 378                 while( H -- ) {
 379                         memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px);
 380                         dst += scrnpitch;
 381                         src += scrnpitch;
 382                 }
 383         }
 384         //Log("Vesa_2D_Blit: RETURN");
 385 }
 386
 387
 388 // --- Disk Driver Helpers ---
 389 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
 390         tDrvUtil_Callback ReadBlocks, Uint64 BlockSize, Uint Argument)
 391 {
 392         Uint8   tmp[BlockSize]; // C99
 393         Uint64  block = Start / BlockSize;
 394          int    offset = Start - block * BlockSize;
 395          int    leading = BlockSize - offset;
 396         Uint64  num;
 397          int    tailings;
 398         Uint64  ret;
 399
 400         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize xArgument",
 401                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
 402
 403         // Non aligned start, let's fix that!
 404         if(offset != 0)
 405         {
 406                 if(leading > Length)    leading = Length;
 407                 LOG("Reading %i bytes from Block1+%i", leading, offset);
 408                 ret = ReadBlocks(block, 1, tmp, Argument);
 409                 if(ret != 1) {
 410                         LEAVE('i', 0);
 411                         return 0;
 412                 }
 413                 memcpy( Buffer, &tmp[offset], leading );
 414
 415                 if(leading == Length) {
 416                         LEAVE('i', leading);
 417                         return leading;
 418                 }
 419
 420                 Buffer = (Uint8*)Buffer + leading;
 421                 block ++;
 422                 num = ( Length - leading ) / BlockSize;
 423                 tailings = Length - num * BlockSize - leading;
 424         }
 425         else {
 426                 num = Length / BlockSize;
 427                 tailings = Length % BlockSize;
 428         }
 429
 430         // Read central blocks
 431         if(num)
 432         {
 433                 LOG("Reading %i blocks", num);
 434                 ret = ReadBlocks(block, num, Buffer, Argument);
 435                 if(ret != num ) {
 436                         LEAVE('X', leading + ret * BlockSize);
 437                         return leading + ret * BlockSize;
 438                 }
 439         }
 440
 441         // Read last tailing block
 442         if(tailings != 0)
 443         {
 444                 LOG("Reading %i bytes from last block", tailings);
 445                 block += num;
 446                 Buffer = (Uint8*)Buffer + num * BlockSize;
 447                 ret = ReadBlocks(block, 1, tmp, Argument);
 448                 if(ret != 1) {
 449                         LEAVE('X', leading + num * BlockSize);
 450                         return leading + num * BlockSize;
 451                 }
 452                 memcpy( Buffer, tmp, tailings );
 453         }
 454
 455         LEAVE('X', Length);
 456         return Length;
 457 }
 458
 459 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, void *Buffer,
 460         tDrvUtil_Callback ReadBlocks, tDrvUtil_Callback WriteBlocks,
 461         Uint64 BlockSize, Uint Argument)
 462 {
 463         Uint8   tmp[BlockSize]; // C99
 464         Uint64  block = Start / BlockSize;
 465          int    offset = Start - block * BlockSize;
 466          int    leading = BlockSize - offset;
 467         Uint64  num;
 468          int    tailings;
 469         Uint64  ret;
 470
 471         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
 472                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
 473
 474         // Non aligned start, let's fix that!
 475         if(offset != 0)
 476         {
 477                 if(leading > Length)    leading = Length;
 478                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 479                 // Read a copy of the block
 480                 ret = ReadBlocks(block, 1, tmp, Argument);
 481                 if(ret != 1) {
 482                         LEAVE('i', 0);
 483                         return 0;
 484                 }
 485                 // Modify
 486                 memcpy( &tmp[offset], Buffer, leading );
 487                 // Write Back
 488                 ret = WriteBlocks(block, 1, tmp, Argument);
 489                 if(ret != 1) {
 490                         LEAVE('i', 0);
 491                         return 0;
 492                 }
 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("Writing %i blocks", num);
 513                 ret = WriteBlocks(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("Writing %i bytes to last block", tailings);
 524                 block += num;
 525                 Buffer = (Uint8*)Buffer + num * BlockSize;
 526                 // Read
 527                 ret = ReadBlocks(block, 1, tmp, Argument);
 528                 if(ret != 1) {
 529                         LEAVE('X', leading + num * BlockSize);
 530                         return leading + num * BlockSize;
 531                 }
 532                 // Modify
 533                 memcpy( tmp, Buffer, tailings );
 534                 // Write
 535                 ret = WriteBlocks(block, 1, tmp, Argument);
 536                 if(ret != 1) {
 537                         LEAVE('X', leading + num * BlockSize);
 538                         return leading + num * BlockSize;
 539                 }
 540
 541         }
 542
 543         LEAVE('X', Length);
 544         return Length;
 545 }