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 / 8;
 113                  int    widthInChars = FBInfo->Width/giVT_CharWidth;
 114                  int    heightInChars = FBInfo->Height/giVT_CharHeight;
 115                  int    x, y, i;
 116
 117                 Length /= sizeof(tVT_Char);     Offset /= sizeof(tVT_Char);
 118
 119                 x = Offset % widthInChars;      y = Offset / widthInChars;
 120
 121                 // Sanity Check
 122                 if(Offset > heightInChars * widthInChars)       LEAVE_RET('i', 0);
 123                 if(y >= heightInChars)  LEAVE_RET('i', 0);
 124
 125                 if( Offset + Length > heightInChars*widthInChars )
 126                 {
 127                         Length = heightInChars*widthInChars - Offset;
 128                 }
 129
 130                 dest = FBInfo->Framebuffer;
 131                 dest += y * giVT_CharHeight * FBInfo->Pitch;
 132
 133                 for( i = 0; i < Length; i++ )
 134                 {
 135                         if( y >= heightInChars )
 136                         {
 137                                 Log_Notice("DrvUtil", "Stopped at %i", i);
 138                                 break;
 139                         }
 140
 141                         VT_Font_Render(
 142                                 chars->Ch,
 143                                 dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch,
 144                                 VT_Colour12toN(chars->BGCol, FBInfo->Depth),
 145                                 VT_Colour12toN(chars->FGCol, FBInfo->Depth)
 146                                 );
 147
 148                         chars ++;
 149                         x ++;
 150                         if( x >= widthInChars )
 151                         {
 152                                 x = 0;
 153                                 y ++;
 154                                 dest += FBInfo->Pitch*giVT_CharHeight;
 155                         }
 156                 }
 157                 Length = i * sizeof(tVT_Char);
 158                 }
 159                 break;
 160
 161         case VIDEO_BUFFMT_FRAMEBUFFER:
 162                 if(FBInfo->Width*FBInfo->Height*4 < Offset+Length)
 163                 {
 164                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow");
 165                         return 0;
 166                 }
 167
 168                 //TODO: Handle non 32-bpp framebuffer modes
 169                 if( FBInfo->Depth != 32 ) {
 170                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Don't support non 32-bpp FB mode");
 171                         return 0;
 172                 }
 173
 174
 175                 //TODO: Handle pitch != Width*BytesPerPixel
 176                 // Copy to Frambuffer
 177                 dest = (Uint8 *)FBInfo->Framebuffer + Offset;
 178                 memcpy(dest, Buffer, Length);
 179                 break;
 180
 181         case VIDEO_BUFFMT_2DSTREAM:
 182                 Length = DrvUtil_Video_2DStream(
 183                         FBInfo, Buffer, Length,
 184                         &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions)
 185                         );
 186                 break;
 187
 188         default:
 189                 LEAVE('i', -1);
 190                 return -1;
 191         }
 192         LEAVE('x', Length);
 193         return Length;
 194 }
 195
 196 void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour)
 197 {
 198         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 199
 200         // TODO: Handle non-32bit modes
 201         if( FBInfo->Depth != 32 )       return;
 202
 203         // TODO: Be less hacky
 204          int    pitch = FBInfo->Pitch/4;
 205         Uint32  *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X;
 206         while( H -- ) {
 207                 Uint32 *tmp;
 208                  int    i;
 209                 tmp = buf;
 210                 for(i=W;i--;tmp++)      *tmp = Colour;
 211                 buf += pitch;
 212         }
 213 }
 214
 215 void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H)
 216 {
 217         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
 218          int    scrnpitch = FBInfo->Pitch;
 219          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
 220          int    dst = DstY*scrnpitch + DstX;
 221          int    src = SrcY*scrnpitch + SrcX;
 222          int    tmp;
 223
 224         //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)",
 225         //      Ent, DstX, DstY, SrcX, SrcY, W, H);
 226
 227         if(SrcX + W > FBInfo->Width)    W = FBInfo->Width - SrcX;
 228         if(DstX + W > FBInfo->Width)    W = FBInfo->Width - DstX;
 229         if(SrcY + H > FBInfo->Height)   H = FBInfo->Height - SrcY;
 230         if(DstY + H > FBInfo->Height)   H = FBInfo->Height - DstY;
 231
 232         //Debug("W = %i, H = %i", W, H);
 233
 234         if( dst > src ) {
 235                 // Reverse copy
 236                 dst += H*scrnpitch;
 237                 src += H*scrnpitch;
 238                 while( H -- ) {
 239                         dst -= scrnpitch;
 240                         src -= scrnpitch;
 241                         tmp = W*bytes_per_px;
 242                         for( tmp = W; tmp --; ) {
 243                                 *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp);
 244                         }
 245                 }
 246         }
 247         else {
 248                 // Normal copy is OK
 249                 while( H -- ) {
 250                         memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px);
 251                         dst += scrnpitch;
 252                         src += scrnpitch;
 253                 }
 254         }
 255         //Log("Vesa_2D_Blit: RETURN");
 256 }
 257
 258
 259 // --- Disk Driver Helpers ---
 260 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
 261         tDrvUtil_Callback ReadBlocks, Uint64 BlockSize, Uint Argument)
 262 {
 263         Uint8   tmp[BlockSize]; // C99
 264         Uint64  block = Start / BlockSize;
 265          int    offset = Start - block * BlockSize;
 266          int    leading = BlockSize - offset;
 267         Uint64  num;
 268          int    tailings;
 269         Uint64  ret;
 270
 271         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize xArgument",
 272                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
 273
 274         // Non aligned start, let's fix that!
 275         if(offset != 0)
 276         {
 277                 if(leading > Length)    leading = Length;
 278                 LOG("Reading %i bytes from Block1+%i", leading, offset);
 279                 ret = ReadBlocks(block, 1, tmp, Argument);
 280                 if(ret != 1) {
 281                         LEAVE('i', 0);
 282                         return 0;
 283                 }
 284                 memcpy( Buffer, &tmp[offset], leading );
 285
 286                 if(leading == Length) {
 287                         LEAVE('i', leading);
 288                         return leading;
 289                 }
 290
 291                 Buffer = (Uint8*)Buffer + leading;
 292                 block ++;
 293                 num = ( Length - leading ) / BlockSize;
 294                 tailings = Length - num * BlockSize - leading;
 295         }
 296         else {
 297                 num = Length / BlockSize;
 298                 tailings = Length % BlockSize;
 299         }
 300
 301         // Read central blocks
 302         if(num)
 303         {
 304                 LOG("Reading %i blocks", num);
 305                 ret = ReadBlocks(block, num, Buffer, Argument);
 306                 if(ret != num ) {
 307                         LEAVE('X', leading + ret * BlockSize);
 308                         return leading + ret * BlockSize;
 309                 }
 310         }
 311
 312         // Read last tailing block
 313         if(tailings != 0)
 314         {
 315                 LOG("Reading %i bytes from last block", tailings);
 316                 block += num;
 317                 Buffer = (Uint8*)Buffer + num * BlockSize;
 318                 ret = ReadBlocks(block, 1, tmp, Argument);
 319                 if(ret != 1) {
 320                         LEAVE('X', leading + num * BlockSize);
 321                         return leading + num * BlockSize;
 322                 }
 323                 memcpy( Buffer, tmp, tailings );
 324         }
 325
 326         LEAVE('X', Length);
 327         return Length;
 328 }
 329
 330 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, void *Buffer,
 331         tDrvUtil_Callback ReadBlocks, tDrvUtil_Callback WriteBlocks,
 332         Uint64 BlockSize, Uint Argument)
 333 {
 334         Uint8   tmp[BlockSize]; // C99
 335         Uint64  block = Start / BlockSize;
 336          int    offset = Start - block * BlockSize;
 337          int    leading = BlockSize - offset;
 338         Uint64  num;
 339          int    tailings;
 340         Uint64  ret;
 341
 342         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
 343                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
 344
 345         // Non aligned start, let's fix that!
 346         if(offset != 0)
 347         {
 348                 if(leading > Length)    leading = Length;
 349                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 350                 // Read a copy of the block
 351                 ret = ReadBlocks(block, 1, tmp, Argument);
 352                 if(ret != 1) {
 353                         LEAVE('i', 0);
 354                         return 0;
 355                 }
 356                 // Modify
 357                 memcpy( &tmp[offset], Buffer, leading );
 358                 // Write Back
 359                 ret = WriteBlocks(block, 1, tmp, Argument);
 360                 if(ret != 1) {
 361                         LEAVE('i', 0);
 362                         return 0;
 363                 }
 364
 365                 if(leading == Length) {
 366                         LEAVE('i', leading);
 367                         return leading;
 368                 }
 369
 370                 Buffer = (Uint8*)Buffer + leading;
 371                 block ++;
 372                 num = ( Length - leading ) / BlockSize;
 373                 tailings = Length - num * BlockSize - leading;
 374         }
 375         else {
 376                 num = Length / BlockSize;
 377                 tailings = Length % BlockSize;
 378         }
 379
 380         // Read central blocks
 381         if(num)
 382         {
 383                 LOG("Writing %i blocks", num);
 384                 ret = WriteBlocks(block, num, Buffer, Argument);
 385                 if(ret != num ) {
 386                         LEAVE('X', leading + ret * BlockSize);
 387                         return leading + ret * BlockSize;
 388                 }
 389         }
 390
 391         // Read last tailing block
 392         if(tailings != 0)
 393         {
 394                 LOG("Writing %i bytes to last block", tailings);
 395                 block += num;
 396                 Buffer = (Uint8*)Buffer + num * BlockSize;
 397                 // Read
 398                 ret = ReadBlocks(block, 1, tmp, Argument);
 399                 if(ret != 1) {
 400                         LEAVE('X', leading + num * BlockSize);
 401                         return leading + num * BlockSize;
 402                 }
 403                 // Modify
 404                 memcpy( tmp, Buffer, tailings );
 405                 // Write
 406                 ret = WriteBlocks(block, 1, tmp, Argument);
 407                 if(ret != 1) {
 408                         LEAVE('X', leading + num * BlockSize);
 409                         return leading + num * BlockSize;
 410                 }
 411
 412         }
 413
 414         LEAVE('X', Length);
 415         return Length;
 416 }