From: John Hodge Date: Fri, 13 Jul 2012 05:24:05 +0000 (+0800) Subject: Kernel - Split drvutil.c into disk and video variants X-Git-Tag: rel0.15~611^2~41^2~8 X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;h=15235fb0b42c91513c30117155dc1b5a020baf76;p=tpg%2Facess2.git Kernel - Split drvutil.c into disk and video variants --- diff --git a/KernelLand/Kernel/Makefile b/KernelLand/Kernel/Makefile index 153fcafa..42b47dcf 100644 --- a/KernelLand/Kernel/Makefile +++ b/KernelLand/Kernel/Makefile @@ -51,7 +51,8 @@ BUILDINFO_OBJ := $(OBJDIR)buildinfo.o$(OBJSUFFIX) BUILDINFO_SRC := $(OBJDIR)buildinfo.c$(OBJSUFFIX) OBJ := $(addprefix arch/$(ARCHDIR)/,$(A_OBJ)) -OBJ += heap.o drvutil.o logging.o debug.o lib.o libc.o adt.o time.o +OBJ += heap.o logging.o debug.o lib.o libc.o adt.o time.o +OBJ += drvutil_video.o drvutil_disk.o OBJ += messages.o modules.o syscalls.o system.o OBJ += threads.o mutex.o semaphore.o workqueue.o events.o OBJ += drv/proc.o drv/fifo.o drv/iocache.o drv/pci.o diff --git a/KernelLand/Kernel/drvutil.c b/KernelLand/Kernel/drvutil.c deleted file mode 100644 index d2c86bfb..00000000 --- a/KernelLand/Kernel/drvutil.c +++ /dev/null @@ -1,740 +0,0 @@ -/* - * Acess2 Kernel - * - By John Hodge - * - * drvutil.c - * - Common Driver/Filesystem Helper Functions - */ -#define DEBUG 0 -#include -#include -#include - -// === TYPES === - -// === PROTOTYPES === -//int DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length, tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers); -//size_t DrvUtil_Video_WriteLFB(int Mode, tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, void *Src); -//void DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap); -//void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y); -void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf); -//void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf); -void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour); -void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H); - -// === GLOBALS === -tDrvUtil_Video_2DHandlers gDrvUtil_Stub_2DFunctions = { - NULL, - DrvUtil_Video_2D_Fill, - DrvUtil_Video_2D_Blit -}; -tVideo_IOCtl_Bitmap gDrvUtil_TextModeCursor = { - 8, 16, - 0, 0, - { - 0, 0, 0 , 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0,-1, 0xFF000000, 0, 0, 0, 0, 0, - 0, 0, 0 , 0, 0, 0, 0, 0, - 0, 0, 0 , 0, 0, 0, 0, 0 - } -}; - -// === CODE === -// --- Video Driver Helpers --- -int DrvUtil_Video_2DStream(void *Ent, const void *Buffer, int Length, - tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers) -{ - const Uint8 *stream = Buffer; - int rem = Length; - int op; - while( rem ) - { - rem --; - op = *stream; - stream ++; - - if(op > NUM_VIDEO_2DOPS) { - Log_Warning("DrvUtil", - "DrvUtil_Video_2DStream: Unknown operation %i", - op); - return Length-rem; - } - - if(op*sizeof(void*) > SizeofHandlers) { - Log_Warning("DrvUtil", - "DrvUtil_Video_2DStream: Driver does not support op %i", - op); - return Length-rem; - } - - switch(op) - { - case VIDEO_2DOP_NOP: break; - - case VIDEO_2DOP_FILL: - if(rem < 10) return Length-rem; - - if(!Handlers->Fill) { - Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver" - " does not support VIDEO_2DOP_FILL"); - return Length-rem; - } - - Handlers->Fill( - Ent, - ((const Uint16*)stream)[0], ((const Uint16*)stream)[1], - ((const Uint16*)stream)[2], ((const Uint16*)stream)[3], - ((const Uint32*)stream)[4] - ); - - rem -= 10; - stream += 10; - break; - - case VIDEO_2DOP_BLIT: - if(rem < 12) return Length-rem; - - if(!Handlers->Blit) { - Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver" - " does not support VIDEO_2DOP_BLIT"); - return Length-rem; - } - - Handlers->Blit( - Ent, - ((const Uint16*)stream)[0], ((const Uint16*)stream)[1], - ((const Uint16*)stream)[2], ((const Uint16*)stream)[3], - ((const Uint16*)stream)[4], ((const Uint16*)stream)[5] - ); - - rem -= 12; - stream += 12; - break; - - } - } - return 0; -} - -int DrvUtil_Video_WriteLFB(tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, const void *Buffer) -{ - Uint8 *dest; - const Uint32 *src = Buffer; - int csr_x, csr_y; - int x, y; - int bytes_per_px = (FBInfo->Depth + 7) / 8; - ENTER("pFBInfo xOffset xLength pBuffer", - FBInfo, Offset, Length, Buffer); - - csr_x = FBInfo->CursorX; - csr_y = FBInfo->CursorY; - - DrvUtil_Video_RemoveCursor(FBInfo); - - switch( FBInfo->BufferFormat ) - { - case VIDEO_BUFFMT_TEXT: - { - const tVT_Char *chars = Buffer; - int widthInChars = FBInfo->Width/giVT_CharWidth; - int heightInChars = FBInfo->Height/giVT_CharHeight; - int i; - - LOG("bytes_per_px = %i", bytes_per_px); - LOG("widthInChars = %i, heightInChars = %i", widthInChars, heightInChars); - - Length /= sizeof(tVT_Char); Offset /= sizeof(tVT_Char); - - x = Offset % widthInChars; y = Offset / widthInChars; - LOG("x = %i, y = %i", x, y); - - // Sanity Check - if(Offset > heightInChars * widthInChars) LEAVE_RET('i', 0); - if(y >= heightInChars) LEAVE_RET('i', 0); - - if( Offset + Length > heightInChars*widthInChars ) - { - Length = heightInChars*widthInChars - Offset; - } - - dest = FBInfo->Framebuffer; - LOG("dest = %p", dest); - dest += y * giVT_CharHeight * FBInfo->Pitch; - LOG("dest = %p", dest); - - for( i = 0; i < Length; i++ ) - { - if( y >= heightInChars ) - { - Log_Notice("DrvUtil", "Stopped at %i", i); - break; - } - - VT_Font_Render( - chars->Ch, - dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch, - VT_Colour12toN(chars->BGCol, FBInfo->Depth), - VT_Colour12toN(chars->FGCol, FBInfo->Depth) - ); - - chars ++; - x ++; - if( x >= widthInChars ) - { - x = 0; - y ++; - dest += FBInfo->Pitch*giVT_CharHeight; - LOG("dest = %p", dest); - } - } - Length = i * sizeof(tVT_Char); - } - break; - - case VIDEO_BUFFMT_FRAMEBUFFER: - if(FBInfo->Width*FBInfo->Height*4 < Offset+Length) - { - Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow"); - return 0; - } - - switch(FBInfo->Depth) - { - case 15: - case 16: - Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in LFB write"); - break; - case 24: - x = Offset % FBInfo->Width; - y = Offset / FBInfo->Width; - dest = (Uint8*)FBInfo->Framebuffer + y*FBInfo->Pitch; - for( ; Length >= 4; Length -= 4 ) - { - dest[x*3+0] = *src & 0xFF; - dest[x*3+1] = (*src >> 8) & 0xFF; - dest[x*3+2] = (*src >> 16) & 0xFF; - x ++; - if(x == FBInfo->Width) { - dest += FBInfo->Pitch; - x = 0; - } - } - break; - case 32: - // Copy to Frambuffer - if( FBInfo->Pitch != FBInfo->Width*4 ) - { - Uint32 *px; - // Pitch isn't 4*Width - x = Offset % FBInfo->Width; - y = Offset / FBInfo->Height; - - px = (Uint32*)FBInfo->Framebuffer + y*FBInfo->Pitch/4; - - for( ; Length >= 4; Length -= 4, x ) - { - px[x++] = *src ++; - if( x == FBInfo->Width ) { - x = 0; - px += FBInfo->Pitch; - } - } - } - else - { - dest = (Uint8 *)FBInfo->Framebuffer + Offset; - memcpy(dest, Buffer, Length); - } - break; - default: - Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Unknown bit depthn %i", FBInfo->Depth); - break; - } - break; - - case VIDEO_BUFFMT_2DSTREAM: - Length = DrvUtil_Video_2DStream( - FBInfo, Buffer, Length, - &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions) - ); - break; - - default: - LEAVE('i', -1); - return -1; - } - - DrvUtil_Video_DrawCursor(FBInfo, csr_x, csr_y); - - LEAVE('x', Length); - return Length; -} - -int DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap) -{ - int csrX = Buf->CursorX, csrY = Buf->CursorY; - size_t size; - - ENTER("pBuf pBitmap", Buf, Bitmap); - - // Clear old bitmap - if( Buf->CursorBitmap ) - { - LOG("Clearing old cursor"); - DrvUtil_Video_RemoveCursor(Buf); - if( !Bitmap || Bitmap->W != Buf->CursorBitmap->W || Bitmap->H != Buf->CursorBitmap->H ) - { - free( Buf->CursorSaveBuf ); - Buf->CursorSaveBuf = NULL; - } - if( Buf->CursorBitmap != &gDrvUtil_TextModeCursor) - free(Buf->CursorBitmap); - Buf->CursorBitmap = NULL; - } - - // If the new bitmap is null, disable drawing - if( !Bitmap ) - { - Buf->CursorX = -1; - Buf->CursorY = -1; - LEAVE('i', 0); - return 0; - } - - // Sanity check the bitmap - LOG("Sanity checking plox"); - if( !CheckMem(Bitmap, sizeof(*Bitmap)) || !CheckMem(Bitmap->Data, Bitmap->W*Bitmap->H*sizeof(Uint32)) ) - { - Log_Warning("DrvUtil", "DrvUtil_Video_SetCursor: Bitmap (%p) is in invalid memory", Bitmap); - errno = -EINVAL; - LEAVE('i', -1); - return -1; - } - - // Don't take a copy of the DrvUtil provided cursor - if( Bitmap == &gDrvUtil_TextModeCursor ) - { - LOG("No copy (provided cursor)"); - Buf->CursorBitmap = Bitmap; - } - else - { - LOG("Make copy"); - size = sizeof(tVideo_IOCtl_Bitmap) + Bitmap->W*Bitmap->H*4; - - // Take a copy - Buf->CursorBitmap = malloc( size ); - memcpy(Buf->CursorBitmap, Bitmap, size); - } - - // Restore cursor position - LOG("Drawing"); - DrvUtil_Video_DrawCursor(Buf, csrX, csrY); - LEAVE('i', 0); - return 0; -} - -void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y) -{ - int render_ox=0, render_oy=0, render_w, render_h; - - ENTER("pBuf iX iY", Buf, X, Y); - DrvUtil_Video_RemoveCursor(Buf); - - // X < 0 disables the cursor - if( X < 0 ) { - Buf->CursorX = -1; - LEAVE('-'); - return ; - } - - // Sanity checking - if( X < 0 || Y < 0 || X >= Buf->Width || Y >= Buf->Height ) { - LEAVE('-'); - return ; - } - - // Ensure the cursor is enabled - if( !Buf->CursorBitmap ) { - LEAVE('-'); - return ; - } - - // Save cursor position (for changing the bitmap) - Buf->CursorX = X; Buf->CursorY = Y; - // Apply cursor's center offset - X -= Buf->CursorBitmap->XOfs; - Y -= Buf->CursorBitmap->YOfs; - - // Get the width of the cursor on screen (clipping to right/bottom edges) - render_w = X > Buf->Width - Buf->CursorBitmap->W ? Buf->Width - X : Buf->CursorBitmap->W; - render_h = Y > Buf->Height - Buf->CursorBitmap->H ? Buf->Height - Y : Buf->CursorBitmap->H; - - // Clipp to left/top edges - if(X < 0) { render_ox = -X; X = 0; } - if(Y < 0) { render_oy = -Y; Y = 0; } - - // Save values - Buf->CursorRenderW = render_w; Buf->CursorRenderH = render_h; - Buf->CursorDestX = X; Buf->CursorDestY = Y; - Buf->CursorReadX = render_ox; Buf->CursorReadY = render_oy; - - LOG("%ix%i at %i,%i offset %i,%i", - render_w, render_h, X, Y, render_ox, render_oy); - - // Call render routine - DrvUtil_Video_RenderCursor(Buf); - LEAVE('-'); -} - -void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf) -{ - int src_x = Buf->CursorReadX, src_y = Buf->CursorReadY; - int render_w = Buf->CursorRenderW, render_h = Buf->CursorRenderH; - int dest_x = Buf->CursorDestX, dest_y = Buf->CursorDestY; - int bytes_per_px = (Buf->Depth + 7) / 8; - int save_pitch = Buf->CursorBitmap->W * bytes_per_px; - void *dest; - Uint32 *src; - int x, y; - - dest = (Uint8*)Buf->Framebuffer + dest_y*Buf->Pitch + dest_x*bytes_per_px; - src = Buf->CursorBitmap->Data + src_y * Buf->CursorBitmap->W + src_x; - - LOG("dest = %p, src = %p", dest, src); - - // Allocate save buffer if not already - if( !Buf->CursorSaveBuf ) - Buf->CursorSaveBuf = malloc( Buf->CursorBitmap->W*Buf->CursorBitmap->H*bytes_per_px ); - - LOG("Saving back"); - // Save behind the cursor - for( y = 0; y < render_h; y ++ ) - memcpy( - (Uint8*)Buf->CursorSaveBuf + y*save_pitch, - (Uint8*)dest + y*Buf->Pitch, - render_w*bytes_per_px - ); - - // Draw the cursor - switch(Buf->Depth) - { - case 15: - case 16: - Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in cursor draw"); - break; - case 24: - LOG("24-bit render"); - for( y = 0; y < render_h; y ++ ) - { - Uint8 *px; - px = dest; - for(x = 0; x < render_w; x ++, px += 3) - { - Uint32 value = src[x]; - // TODO: Should I implement alpha blending? - if(value & 0xFF000000) - { - px[0] = value & 0xFF; - px[1] = (value >> 8) & 0xFF; - px[2] = (value >> 16) & 0xFF; - } - else - ; - } - src += Buf->CursorBitmap->W; - dest = (Uint8*)dest + Buf->Pitch; - } - break; - case 32: - LOG("32-bit render"); - for( y = 0; y < render_h; y ++ ) - { - Uint32 *px; - px = dest; - for(x = 0; x < render_w; x ++, px ++) - { - Uint32 value = src[x]; - // TODO: Should I implement alpha blending? - if(value & 0xFF000000) - *px = value; - else - ; // NOP, completely transparent - } - LOG("row %i/%i (%p-%P) done", y+1, render_h, dest, MM_GetPhysAddr(dest)); - src += Buf->CursorBitmap->W; - dest = (Uint8*)dest + Buf->Pitch; - } - break; - default: - Log_Error("DrvUtil", "RenderCursor - Unknown bit depth %i", Buf->Depth); - Buf->CursorX = -1; - break; - } -} - -void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf) -{ - int bytes_per_px = (Buf->Depth + 7) / 8; - int y, save_pitch; - Uint8 *dest, *src; - - // Just a little sanity - if( !Buf->CursorBitmap || Buf->CursorX == -1 ) return ; - if( !Buf->CursorSaveBuf ) return ; - -// Debug("DrvUtil_Video_RemoveCursor: (Buf=%p) dest_x=%i, dest_y=%i", Buf, Buf->CursorDestX, Buf->CursorDestY); - - // Set up - save_pitch = Buf->CursorBitmap->W * bytes_per_px; - dest = (Uint8*)Buf->Framebuffer + Buf->CursorDestY * Buf->Pitch + Buf->CursorDestX*bytes_per_px; - src = Buf->CursorSaveBuf; - - // Copy each line back - for( y = 0; y < Buf->CursorRenderH; y ++ ) - { - memcpy( dest, src, Buf->CursorRenderW * bytes_per_px ); - src += save_pitch; - dest += Buf->Pitch; - } - - // Set the cursor as removed - Buf->CursorX = -1; -} - -void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour) -{ - tDrvUtil_Video_BufInfo *FBInfo = Ent; - - // TODO: Handle non-32bit modes - if( FBInfo->Depth != 32 ) return; - - // TODO: Be less hacky - int pitch = FBInfo->Pitch/4; - Uint32 *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X; - while( H -- ) { - Uint32 *tmp; - int i; - tmp = buf; - for(i=W;i--;tmp++) *tmp = Colour; - buf += pitch; - } -} - -void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H) -{ - tDrvUtil_Video_BufInfo *FBInfo = Ent; - int scrnpitch = FBInfo->Pitch; - int bytes_per_px = (FBInfo->Depth + 7) / 8; - int dst = DstY*scrnpitch + DstX; - int src = SrcY*scrnpitch + SrcX; - int tmp; - - //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)", - // Ent, DstX, DstY, SrcX, SrcY, W, H); - - if(SrcX + W > FBInfo->Width) W = FBInfo->Width - SrcX; - if(DstX + W > FBInfo->Width) W = FBInfo->Width - DstX; - if(SrcY + H > FBInfo->Height) H = FBInfo->Height - SrcY; - if(DstY + H > FBInfo->Height) H = FBInfo->Height - DstY; - - //Debug("W = %i, H = %i", W, H); - - if( dst > src ) { - // Reverse copy - dst += H*scrnpitch; - src += H*scrnpitch; - while( H -- ) { - dst -= scrnpitch; - src -= scrnpitch; - tmp = W*bytes_per_px; - for( tmp = W; tmp --; ) { - *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp); - } - } - } - else if(W == FBInfo->Width && FBInfo->Pitch == FBInfo->Width*bytes_per_px) { - memmove((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, H*FBInfo->Pitch); - } - else { - // Normal copy is OK - while( H -- ) { - memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px); - dst += scrnpitch; - src += scrnpitch; - } - } - //Log("Vesa_2D_Blit: RETURN"); -} - - -// --- Disk Driver Helpers --- -Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer, - tDrvUtil_Read_Callback ReadBlocks, Uint64 BlockSize, void *Argument) -{ - Uint8 tmp[BlockSize]; // C99 - Uint64 block = Start / BlockSize; - int offset = Start - block * BlockSize; - int leading = BlockSize - offset; - Uint64 num; - int tailings; - Uint64 ret; - - ENTER("XStart XLength pBuffer pReadBlocks XBlockSize pArgument", - Start, Length, Buffer, ReadBlocks, BlockSize, Argument); - - // Non aligned start, let's fix that! - if(offset != 0) - { - if(leading > Length) leading = Length; - LOG("Reading %i bytes from Block1+%i", leading, offset); - ret = ReadBlocks(block, 1, tmp, Argument); - if(ret != 1) { - LEAVE('i', 0); - return 0; - } - memcpy( Buffer, &tmp[offset], leading ); - - if(leading == Length) { - LEAVE('i', leading); - return leading; - } - - Buffer = (Uint8*)Buffer + leading; - block ++; - num = ( Length - leading ) / BlockSize; - tailings = Length - num * BlockSize - leading; - } - else { - num = Length / BlockSize; - tailings = Length % BlockSize; - } - - // Read central blocks - if(num) - { - LOG("Reading %i blocks", num); - ret = ReadBlocks(block, num, Buffer, Argument); - if(ret != num ) { - LEAVE('X', leading + ret * BlockSize); - return leading + ret * BlockSize; - } - } - - // Read last tailing block - if(tailings != 0) - { - LOG("Reading %i bytes from last block", tailings); - block += num; - Buffer = (Uint8*)Buffer + num * BlockSize; - ret = ReadBlocks(block, 1, tmp, Argument); - if(ret != 1) { - LEAVE('X', leading + num * BlockSize); - return leading + num * BlockSize; - } - memcpy( Buffer, tmp, tailings ); - } - - LEAVE('X', Length); - return Length; -} - -Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, const void *Buffer, - tDrvUtil_Read_Callback ReadBlocks, tDrvUtil_Write_Callback WriteBlocks, - Uint64 BlockSize, void *Argument) -{ - Uint8 tmp[BlockSize]; // C99 - Uint64 block = Start / BlockSize; - int offset = Start - block * BlockSize; - int leading = BlockSize - offset; - Uint64 num; - int tailings; - Uint64 ret; - - ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize pArgument", - Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument); - - // Non aligned start, let's fix that! - if(offset != 0) - { - if(leading > Length) leading = Length; - LOG("Writing %i bytes to Block1+%i", leading, offset); - // Read a copy of the block - ret = ReadBlocks(block, 1, tmp, Argument); - if(ret != 1) { - LEAVE('i', 0); - return 0; - } - // Modify - memcpy( &tmp[offset], Buffer, leading ); - // Write Back - ret = WriteBlocks(block, 1, tmp, Argument); - if(ret != 1) { - LEAVE('i', 0); - return 0; - } - - if(leading == Length) { - LEAVE('i', leading); - return leading; - } - - Buffer = (Uint8*)Buffer + leading; - block ++; - num = ( Length - leading ) / BlockSize; - tailings = Length - num * BlockSize - leading; - } - else { - num = Length / BlockSize; - tailings = Length % BlockSize; - } - - // Read central blocks - if(num) - { - LOG("Writing %i blocks", num); - ret = WriteBlocks(block, num, Buffer, Argument); - if(ret != num ) { - LEAVE('X', leading + ret * BlockSize); - return leading + ret * BlockSize; - } - } - - // Read last tailing block - if(tailings != 0) - { - LOG("Writing %i bytes to last block", tailings); - block += num; - Buffer = (Uint8*)Buffer + num * BlockSize; - // Read - ret = ReadBlocks(block, 1, tmp, Argument); - if(ret != 1) { - LEAVE('X', leading + num * BlockSize); - return leading + num * BlockSize; - } - // Modify - memcpy( tmp, Buffer, tailings ); - // Write - ret = WriteBlocks(block, 1, tmp, Argument); - if(ret != 1) { - LEAVE('X', leading + num * BlockSize); - return leading + num * BlockSize; - } - - } - - LEAVE('X', Length); - return Length; -} diff --git a/KernelLand/Kernel/drvutil_disk.c b/KernelLand/Kernel/drvutil_disk.c new file mode 100644 index 00000000..db4d631a --- /dev/null +++ b/KernelLand/Kernel/drvutil_disk.c @@ -0,0 +1,169 @@ +/* + * Acess2 Kernel + * - By John Hodge + * + * drvutil_disk.c + * - Storage Driver Helper Functions + */ +#define DEBUG 0 +#include +#include + +// --- Disk Driver Helpers --- +Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer, + tDrvUtil_Read_Callback ReadBlocks, Uint64 BlockSize, void *Argument) +{ + Uint8 tmp[BlockSize]; // C99 + Uint64 block = Start / BlockSize; + int offset = Start - block * BlockSize; + int leading = BlockSize - offset; + Uint64 num; + int tailings; + Uint64 ret; + + ENTER("XStart XLength pBuffer pReadBlocks XBlockSize pArgument", + Start, Length, Buffer, ReadBlocks, BlockSize, Argument); + + // Non aligned start, let's fix that! + if(offset != 0) + { + if(leading > Length) leading = Length; + LOG("Reading %i bytes from Block1+%i", leading, offset); + ret = ReadBlocks(block, 1, tmp, Argument); + if(ret != 1) { + LEAVE('i', 0); + return 0; + } + memcpy( Buffer, &tmp[offset], leading ); + + if(leading == Length) { + LEAVE('i', leading); + return leading; + } + + Buffer = (Uint8*)Buffer + leading; + block ++; + num = ( Length - leading ) / BlockSize; + tailings = Length - num * BlockSize - leading; + } + else { + num = Length / BlockSize; + tailings = Length % BlockSize; + } + + // Read central blocks + if(num) + { + LOG("Reading %i blocks", num); + ret = ReadBlocks(block, num, Buffer, Argument); + if(ret != num ) { + LEAVE('X', leading + ret * BlockSize); + return leading + ret * BlockSize; + } + } + + // Read last tailing block + if(tailings != 0) + { + LOG("Reading %i bytes from last block", tailings); + block += num; + Buffer = (Uint8*)Buffer + num * BlockSize; + ret = ReadBlocks(block, 1, tmp, Argument); + if(ret != 1) { + LEAVE('X', leading + num * BlockSize); + return leading + num * BlockSize; + } + memcpy( Buffer, tmp, tailings ); + } + + LEAVE('X', Length); + return Length; +} + +Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, const void *Buffer, + tDrvUtil_Read_Callback ReadBlocks, tDrvUtil_Write_Callback WriteBlocks, + Uint64 BlockSize, void *Argument) +{ + Uint8 tmp[BlockSize]; // C99 + Uint64 block = Start / BlockSize; + int offset = Start - block * BlockSize; + int leading = BlockSize - offset; + Uint64 num; + int tailings; + Uint64 ret; + + ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize pArgument", + Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument); + + // Non aligned start, let's fix that! + if(offset != 0) + { + if(leading > Length) leading = Length; + LOG("Writing %i bytes to Block1+%i", leading, offset); + // Read a copy of the block + ret = ReadBlocks(block, 1, tmp, Argument); + if(ret != 1) { + LEAVE('i', 0); + return 0; + } + // Modify + memcpy( &tmp[offset], Buffer, leading ); + // Write Back + ret = WriteBlocks(block, 1, tmp, Argument); + if(ret != 1) { + LEAVE('i', 0); + return 0; + } + + if(leading == Length) { + LEAVE('i', leading); + return leading; + } + + Buffer = (Uint8*)Buffer + leading; + block ++; + num = ( Length - leading ) / BlockSize; + tailings = Length - num * BlockSize - leading; + } + else { + num = Length / BlockSize; + tailings = Length % BlockSize; + } + + // Read central blocks + if(num) + { + LOG("Writing %i blocks", num); + ret = WriteBlocks(block, num, Buffer, Argument); + if(ret != num ) { + LEAVE('X', leading + ret * BlockSize); + return leading + ret * BlockSize; + } + } + + // Read last tailing block + if(tailings != 0) + { + LOG("Writing %i bytes to last block", tailings); + block += num; + Buffer = (Uint8*)Buffer + num * BlockSize; + // Read + ret = ReadBlocks(block, 1, tmp, Argument); + if(ret != 1) { + LEAVE('X', leading + num * BlockSize); + return leading + num * BlockSize; + } + // Modify + memcpy( tmp, Buffer, tailings ); + // Write + ret = WriteBlocks(block, 1, tmp, Argument); + if(ret != 1) { + LEAVE('X', leading + num * BlockSize); + return leading + num * BlockSize; + } + + } + + LEAVE('X', Length); + return Length; +} diff --git a/KernelLand/Kernel/drvutil_video.c b/KernelLand/Kernel/drvutil_video.c new file mode 100644 index 00000000..6e633869 --- /dev/null +++ b/KernelLand/Kernel/drvutil_video.c @@ -0,0 +1,581 @@ +/* + * Acess2 Kernel + * - By John Hodge + * + * drvutil.c + * - Video Driver Helper Functions + */ +#define DEBUG 0 +#include +#include + +// === TYPES === + +// === PROTOTYPES === +//int DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length, tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers); +//size_t DrvUtil_Video_WriteLFB(int Mode, tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, void *Src); +//void DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap); +//void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y); +void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf); +//void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf); +void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour); +void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H); + +// === GLOBALS === +tDrvUtil_Video_2DHandlers gDrvUtil_Stub_2DFunctions = { + NULL, + DrvUtil_Video_2D_Fill, + DrvUtil_Video_2D_Blit +}; +tVideo_IOCtl_Bitmap gDrvUtil_TextModeCursor = { + 8, 16, + 0, 0, + { + 0, 0, 0 , 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0,-1, 0xFF000000, 0, 0, 0, 0, 0, + 0, 0, 0 , 0, 0, 0, 0, 0, + 0, 0, 0 , 0, 0, 0, 0, 0 + } +}; + +// === CODE === +// --- Video Driver Helpers --- +int DrvUtil_Video_2DStream(void *Ent, const void *Buffer, int Length, + tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers) +{ + const Uint8 *stream = Buffer; + int rem = Length; + int op; + while( rem ) + { + rem --; + op = *stream; + stream ++; + + if(op > NUM_VIDEO_2DOPS) { + Log_Warning("DrvUtil", + "DrvUtil_Video_2DStream: Unknown operation %i", + op); + return Length-rem; + } + + if(op*sizeof(void*) > SizeofHandlers) { + Log_Warning("DrvUtil", + "DrvUtil_Video_2DStream: Driver does not support op %i", + op); + return Length-rem; + } + + switch(op) + { + case VIDEO_2DOP_NOP: break; + + case VIDEO_2DOP_FILL: + if(rem < 10) return Length-rem; + + if(!Handlers->Fill) { + Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver" + " does not support VIDEO_2DOP_FILL"); + return Length-rem; + } + + Handlers->Fill( + Ent, + ((const Uint16*)stream)[0], ((const Uint16*)stream)[1], + ((const Uint16*)stream)[2], ((const Uint16*)stream)[3], + ((const Uint32*)stream)[4] + ); + + rem -= 10; + stream += 10; + break; + + case VIDEO_2DOP_BLIT: + if(rem < 12) return Length-rem; + + if(!Handlers->Blit) { + Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver" + " does not support VIDEO_2DOP_BLIT"); + return Length-rem; + } + + Handlers->Blit( + Ent, + ((const Uint16*)stream)[0], ((const Uint16*)stream)[1], + ((const Uint16*)stream)[2], ((const Uint16*)stream)[3], + ((const Uint16*)stream)[4], ((const Uint16*)stream)[5] + ); + + rem -= 12; + stream += 12; + break; + + } + } + return 0; +} + +int DrvUtil_Video_WriteLFB(tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, const void *Buffer) +{ + Uint8 *dest; + const Uint32 *src = Buffer; + int csr_x, csr_y; + int x, y; + int bytes_per_px = (FBInfo->Depth + 7) / 8; + ENTER("pFBInfo xOffset xLength pBuffer", + FBInfo, Offset, Length, Buffer); + + csr_x = FBInfo->CursorX; + csr_y = FBInfo->CursorY; + + DrvUtil_Video_RemoveCursor(FBInfo); + + switch( FBInfo->BufferFormat ) + { + case VIDEO_BUFFMT_TEXT: + { + const tVT_Char *chars = Buffer; + int widthInChars = FBInfo->Width/giVT_CharWidth; + int heightInChars = FBInfo->Height/giVT_CharHeight; + int i; + + LOG("bytes_per_px = %i", bytes_per_px); + LOG("widthInChars = %i, heightInChars = %i", widthInChars, heightInChars); + + Length /= sizeof(tVT_Char); Offset /= sizeof(tVT_Char); + + x = Offset % widthInChars; y = Offset / widthInChars; + LOG("x = %i, y = %i", x, y); + + // Sanity Check + if(Offset > heightInChars * widthInChars) LEAVE_RET('i', 0); + if(y >= heightInChars) LEAVE_RET('i', 0); + + if( Offset + Length > heightInChars*widthInChars ) + { + Length = heightInChars*widthInChars - Offset; + } + + dest = FBInfo->Framebuffer; + LOG("dest = %p", dest); + dest += y * giVT_CharHeight * FBInfo->Pitch; + LOG("dest = %p", dest); + + for( i = 0; i < Length; i++ ) + { + if( y >= heightInChars ) + { + Log_Notice("DrvUtil", "Stopped at %i", i); + break; + } + + VT_Font_Render( + chars->Ch, + dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch, + VT_Colour12toN(chars->BGCol, FBInfo->Depth), + VT_Colour12toN(chars->FGCol, FBInfo->Depth) + ); + + chars ++; + x ++; + if( x >= widthInChars ) + { + x = 0; + y ++; + dest += FBInfo->Pitch*giVT_CharHeight; + LOG("dest = %p", dest); + } + } + Length = i * sizeof(tVT_Char); + } + break; + + case VIDEO_BUFFMT_FRAMEBUFFER: + if(FBInfo->Width*FBInfo->Height*4 < Offset+Length) + { + Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow"); + return 0; + } + + switch(FBInfo->Depth) + { + case 15: + case 16: + Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in LFB write"); + break; + case 24: + x = Offset % FBInfo->Width; + y = Offset / FBInfo->Width; + dest = (Uint8*)FBInfo->Framebuffer + y*FBInfo->Pitch; + for( ; Length >= 4; Length -= 4 ) + { + dest[x*3+0] = *src & 0xFF; + dest[x*3+1] = (*src >> 8) & 0xFF; + dest[x*3+2] = (*src >> 16) & 0xFF; + x ++; + if(x == FBInfo->Width) { + dest += FBInfo->Pitch; + x = 0; + } + } + break; + case 32: + // Copy to Frambuffer + if( FBInfo->Pitch != FBInfo->Width*4 ) + { + Uint32 *px; + // Pitch isn't 4*Width + x = Offset % FBInfo->Width; + y = Offset / FBInfo->Height; + + px = (Uint32*)FBInfo->Framebuffer + y*FBInfo->Pitch/4; + + for( ; Length >= 4; Length -= 4, x ) + { + px[x++] = *src ++; + if( x == FBInfo->Width ) { + x = 0; + px += FBInfo->Pitch; + } + } + } + else + { + dest = (Uint8 *)FBInfo->Framebuffer + Offset; + memcpy(dest, Buffer, Length); + } + break; + default: + Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Unknown bit depthn %i", FBInfo->Depth); + break; + } + break; + + case VIDEO_BUFFMT_2DSTREAM: + Length = DrvUtil_Video_2DStream( + FBInfo, Buffer, Length, + &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions) + ); + break; + + default: + LEAVE('i', -1); + return -1; + } + + DrvUtil_Video_DrawCursor(FBInfo, csr_x, csr_y); + + LEAVE('x', Length); + return Length; +} + +int DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap) +{ + int csrX = Buf->CursorX, csrY = Buf->CursorY; + size_t size; + + ENTER("pBuf pBitmap", Buf, Bitmap); + + // Clear old bitmap + if( Buf->CursorBitmap ) + { + LOG("Clearing old cursor"); + DrvUtil_Video_RemoveCursor(Buf); + if( !Bitmap || Bitmap->W != Buf->CursorBitmap->W || Bitmap->H != Buf->CursorBitmap->H ) + { + free( Buf->CursorSaveBuf ); + Buf->CursorSaveBuf = NULL; + } + if( Buf->CursorBitmap != &gDrvUtil_TextModeCursor) + free(Buf->CursorBitmap); + Buf->CursorBitmap = NULL; + } + + // If the new bitmap is null, disable drawing + if( !Bitmap ) + { + Buf->CursorX = -1; + Buf->CursorY = -1; + LEAVE('i', 0); + return 0; + } + + // Sanity check the bitmap + LOG("Sanity checking plox"); + if( !CheckMem(Bitmap, sizeof(*Bitmap)) || !CheckMem(Bitmap->Data, Bitmap->W*Bitmap->H*sizeof(Uint32)) ) + { + Log_Warning("DrvUtil", "DrvUtil_Video_SetCursor: Bitmap (%p) is in invalid memory", Bitmap); + errno = -EINVAL; + LEAVE('i', -1); + return -1; + } + + // Don't take a copy of the DrvUtil provided cursor + if( Bitmap == &gDrvUtil_TextModeCursor ) + { + LOG("No copy (provided cursor)"); + Buf->CursorBitmap = Bitmap; + } + else + { + LOG("Make copy"); + size = sizeof(tVideo_IOCtl_Bitmap) + Bitmap->W*Bitmap->H*4; + + // Take a copy + Buf->CursorBitmap = malloc( size ); + memcpy(Buf->CursorBitmap, Bitmap, size); + } + + // Restore cursor position + LOG("Drawing"); + DrvUtil_Video_DrawCursor(Buf, csrX, csrY); + LEAVE('i', 0); + return 0; +} + +void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y) +{ + int render_ox=0, render_oy=0, render_w, render_h; + + ENTER("pBuf iX iY", Buf, X, Y); + DrvUtil_Video_RemoveCursor(Buf); + + // X < 0 disables the cursor + if( X < 0 ) { + Buf->CursorX = -1; + LEAVE('-'); + return ; + } + + // Sanity checking + if( X < 0 || Y < 0 || X >= Buf->Width || Y >= Buf->Height ) { + LEAVE('-'); + return ; + } + + // Ensure the cursor is enabled + if( !Buf->CursorBitmap ) { + LEAVE('-'); + return ; + } + + // Save cursor position (for changing the bitmap) + Buf->CursorX = X; Buf->CursorY = Y; + // Apply cursor's center offset + X -= Buf->CursorBitmap->XOfs; + Y -= Buf->CursorBitmap->YOfs; + + // Get the width of the cursor on screen (clipping to right/bottom edges) + render_w = X > Buf->Width - Buf->CursorBitmap->W ? Buf->Width - X : Buf->CursorBitmap->W; + render_h = Y > Buf->Height - Buf->CursorBitmap->H ? Buf->Height - Y : Buf->CursorBitmap->H; + + // Clipp to left/top edges + if(X < 0) { render_ox = -X; X = 0; } + if(Y < 0) { render_oy = -Y; Y = 0; } + + // Save values + Buf->CursorRenderW = render_w; Buf->CursorRenderH = render_h; + Buf->CursorDestX = X; Buf->CursorDestY = Y; + Buf->CursorReadX = render_ox; Buf->CursorReadY = render_oy; + + LOG("%ix%i at %i,%i offset %i,%i", + render_w, render_h, X, Y, render_ox, render_oy); + + // Call render routine + DrvUtil_Video_RenderCursor(Buf); + LEAVE('-'); +} + +void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf) +{ + int src_x = Buf->CursorReadX, src_y = Buf->CursorReadY; + int render_w = Buf->CursorRenderW, render_h = Buf->CursorRenderH; + int dest_x = Buf->CursorDestX, dest_y = Buf->CursorDestY; + int bytes_per_px = (Buf->Depth + 7) / 8; + int save_pitch = Buf->CursorBitmap->W * bytes_per_px; + void *dest; + Uint32 *src; + int x, y; + + dest = (Uint8*)Buf->Framebuffer + dest_y*Buf->Pitch + dest_x*bytes_per_px; + src = Buf->CursorBitmap->Data + src_y * Buf->CursorBitmap->W + src_x; + + LOG("dest = %p, src = %p", dest, src); + + // Allocate save buffer if not already + if( !Buf->CursorSaveBuf ) + Buf->CursorSaveBuf = malloc( Buf->CursorBitmap->W*Buf->CursorBitmap->H*bytes_per_px ); + + LOG("Saving back"); + // Save behind the cursor + for( y = 0; y < render_h; y ++ ) + memcpy( + (Uint8*)Buf->CursorSaveBuf + y*save_pitch, + (Uint8*)dest + y*Buf->Pitch, + render_w*bytes_per_px + ); + + // Draw the cursor + switch(Buf->Depth) + { + case 15: + case 16: + Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in cursor draw"); + break; + case 24: + LOG("24-bit render"); + for( y = 0; y < render_h; y ++ ) + { + Uint8 *px; + px = dest; + for(x = 0; x < render_w; x ++, px += 3) + { + Uint32 value = src[x]; + // TODO: Should I implement alpha blending? + if(value & 0xFF000000) + { + px[0] = value & 0xFF; + px[1] = (value >> 8) & 0xFF; + px[2] = (value >> 16) & 0xFF; + } + else + ; + } + src += Buf->CursorBitmap->W; + dest = (Uint8*)dest + Buf->Pitch; + } + break; + case 32: + LOG("32-bit render"); + for( y = 0; y < render_h; y ++ ) + { + Uint32 *px; + px = dest; + for(x = 0; x < render_w; x ++, px ++) + { + Uint32 value = src[x]; + // TODO: Should I implement alpha blending? + if(value & 0xFF000000) + *px = value; + else + ; // NOP, completely transparent + } + LOG("row %i/%i (%p-%P) done", y+1, render_h, dest, MM_GetPhysAddr(dest)); + src += Buf->CursorBitmap->W; + dest = (Uint8*)dest + Buf->Pitch; + } + break; + default: + Log_Error("DrvUtil", "RenderCursor - Unknown bit depth %i", Buf->Depth); + Buf->CursorX = -1; + break; + } +} + +void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf) +{ + int bytes_per_px = (Buf->Depth + 7) / 8; + int y, save_pitch; + Uint8 *dest, *src; + + // Just a little sanity + if( !Buf->CursorBitmap || Buf->CursorX == -1 ) return ; + if( !Buf->CursorSaveBuf ) return ; + +// Debug("DrvUtil_Video_RemoveCursor: (Buf=%p) dest_x=%i, dest_y=%i", Buf, Buf->CursorDestX, Buf->CursorDestY); + + // Set up + save_pitch = Buf->CursorBitmap->W * bytes_per_px; + dest = (Uint8*)Buf->Framebuffer + Buf->CursorDestY * Buf->Pitch + Buf->CursorDestX*bytes_per_px; + src = Buf->CursorSaveBuf; + + // Copy each line back + for( y = 0; y < Buf->CursorRenderH; y ++ ) + { + memcpy( dest, src, Buf->CursorRenderW * bytes_per_px ); + src += save_pitch; + dest += Buf->Pitch; + } + + // Set the cursor as removed + Buf->CursorX = -1; +} + +void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour) +{ + tDrvUtil_Video_BufInfo *FBInfo = Ent; + + // TODO: Handle non-32bit modes + if( FBInfo->Depth != 32 ) return; + + // TODO: Be less hacky + int pitch = FBInfo->Pitch/4; + Uint32 *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X; + while( H -- ) { + Uint32 *tmp; + int i; + tmp = buf; + for(i=W;i--;tmp++) *tmp = Colour; + buf += pitch; + } +} + +void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H) +{ + tDrvUtil_Video_BufInfo *FBInfo = Ent; + int scrnpitch = FBInfo->Pitch; + int bytes_per_px = (FBInfo->Depth + 7) / 8; + int dst = DstY*scrnpitch + DstX; + int src = SrcY*scrnpitch + SrcX; + int tmp; + + //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)", + // Ent, DstX, DstY, SrcX, SrcY, W, H); + + if(SrcX + W > FBInfo->Width) W = FBInfo->Width - SrcX; + if(DstX + W > FBInfo->Width) W = FBInfo->Width - DstX; + if(SrcY + H > FBInfo->Height) H = FBInfo->Height - SrcY; + if(DstY + H > FBInfo->Height) H = FBInfo->Height - DstY; + + //Debug("W = %i, H = %i", W, H); + + if( dst > src ) { + // Reverse copy + dst += H*scrnpitch; + src += H*scrnpitch; + while( H -- ) { + dst -= scrnpitch; + src -= scrnpitch; + tmp = W*bytes_per_px; + for( tmp = W; tmp --; ) { + *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp); + } + } + } + else if(W == FBInfo->Width && FBInfo->Pitch == FBInfo->Width*bytes_per_px) { + memmove((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, H*FBInfo->Pitch); + } + else { + // Normal copy is OK + while( H -- ) { + memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px); + dst += scrnpitch; + src += scrnpitch; + } + } + //Log("Vesa_2D_Blit: RETURN"); +} + +