X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FModules%2FDisplay%2FVESA%2Fmain.c;h=62fb25a2ea82438f428d8b159b251f674b00602b;hb=c1b33e91984102c1aa9a2ffe19f02c315b481726;hp=3f2f1c9cea56a10968f3b2838e5fa226a95be9a6;hpb=47f84632cc84aa1cb625bf5aa90a35539c60eb77;p=tpg%2Facess2.git

diff --git a/KernelLand/Modules/Display/VESA/main.c b/KernelLand/Modules/Display/VESA/main.c
index 3f2f1c9c..62fb25a2 100644
--- a/KernelLand/Modules/Display/VESA/main.c
+++ b/KernelLand/Modules/Display/VESA/main.c
@@ -13,13 +13,16 @@
 #include <vm8086.h>
 #include "common.h"
 #include <timers.h>
+#include <limits.h>
 
 // === CONSTANTS ===
-#define	FLAG_LFB	0x1
-#define FLAG_POPULATED	0x2
-#define FLAG_VALID	0x4
+#ifdef ARCHDIR_is_x86
+# define USE_BIOS	1
+#else
+# define USE_BIOS	0
+#endif
 #define VESA_DEFAULT_FRAMEBUFFER	(KERNEL_BASE|0xA0000)
-#define BLINKING_CURSOR	1
+#define BLINKING_CURSOR	0
 #if BLINKING_CURSOR
 # define VESA_CURSOR_PERIOD	1000
 #endif
@@ -27,14 +30,18 @@
 // === PROTOTYPES ===
  int	Vesa_Install(char **Arguments);
  int	VBE_int_GetModeList(void);
-size_t	Vesa_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer);
+size_t	Vesa_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags);
  int	Vesa_IOCtl(tVFS_Node *Node, int ID, void *Data);
  int	Vesa_Int_SetMode(int Mode);
  int	Vesa_Int_FindMode(tVideo_IOCtl_Mode *data);
  int	Vesa_Int_ModeInfo(tVideo_IOCtl_Mode *data);
 void	Vesa_int_HideCursor(void);
 void	Vesa_int_ShowCursor(void);
+ int	Vesa_int_SetCursor(tVideo_IOCtl_Bitmap *Cursor);
 void	Vesa_FlipCursor(void *Arg);
+Uint16	VBE_int_GetWord(const tVT_Char *Char);
+void	VBE_int_Text_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour);
+void	VBE_int_Text_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H);
 
 // === GLOBALS ===
 MODULE_DEFINE(0, VERSION, Vesa, Vesa_Install, NULL, "PCI", "VM8086", NULL);
@@ -51,8 +58,9 @@ tVM8086	*gpVesa_BiosState;
  int	giVesaDriverId = -1;
 // --- Video Modes ---
  int	giVesaCurrentMode = 0;
+tVesa_Mode	gVesa_BootMode = {0x03, 80*8, 25*16, 80*8*2, 12, FLAG_POPULATED, 80*25*2, 0xB8000};
 tVesa_Mode	*gVesa_Modes;
-tVesa_Mode	*gpVesaCurMode;
+tVesa_Mode	*gpVesaCurMode = &gVesa_BootMode;
  int	giVesaModeCount = 0;
  int	gbVesaModesChecked;
 // --- Framebuffer ---
@@ -67,16 +75,38 @@ tTimer	*gpVesaCursorTimer;
  int	gbVesa_CursorVisible = 0;
 // --- 2D Video Stream Handlers ---
 tDrvUtil_Video_BufInfo	gVesa_BufInfo;
+tDrvUtil_Video_2DHandlers	gVBE_Text2DFunctions = {
+	NULL,
+	VBE_int_Text_2D_Fill,
+	VBE_int_Text_2D_Blit
+};
+// --- Settings ---
+bool	gbVesa_DisableBIOSCalls;	// Disables calls to the BIOS
+// int	gbVesa_DisableFBCache;	// Disables the main-memory framebuffer cache
 
 // === CODE ===
 int Vesa_Install(char **Arguments)
 {
-	 int	rv;
-	
-	gpVesa_BiosState = VM8086_Init();
-	
-	if( (rv = VBE_int_GetModeList()) )
-		return rv;
+	for( int i = 0; Arguments && Arguments[i]; i ++ )
+	{
+		if( strcmp(Arguments[i], "nobios") == 0 )
+			gbVesa_DisableBIOSCalls = 1;
+		//else if( strcmp(Arguments[i], "nocache") == 0 )
+		//	gbVesa_DisableFBCache = 1;
+		else {
+			Log_Notice("VBE", "Unknown argument '%s'", Arguments[i]);
+		}
+	}
+
+	#if USE_BIOS
+	if( !gbVesa_DisableBIOSCalls )
+	{
+		gpVesa_BiosState = VM8086_Init();
+		
+		int rv = VBE_int_GetModeList();
+		if(rv)	return rv;
+	}
+	#endif
 		
 	#if BLINKING_CURSOR
 	// Create blink timer
@@ -90,15 +120,19 @@ int Vesa_Install(char **Arguments)
 	return MODULE_ERR_OK;
 }
 
+#if USE_BIOS
 int VBE_int_GetModeList(void)
 {
 	tVesa_CallInfo	*info;
 	tFarPtr	infoPtr;
 	Uint16	*modes;
-	int	i;
 	
 	// Allocate Info Block
 	info = VM8086_Allocate(gpVesa_BiosState, 512, &infoPtr.seg, &infoPtr.ofs);
+	if( info == NULL ) {
+		Log_Warning("VBE", "VM8086 allocation error");
+		return MODULE_ERR_NOTNEEDED;
+	}
 	// Set Requested Version
 	memcpy(info->signature, "VBE2", 4);
 	// Set Registers
@@ -107,7 +141,7 @@ int VBE_int_GetModeList(void)
 	// Call Interrupt
 	VM8086_Int(gpVesa_BiosState, 0x10);
 	if(gpVesa_BiosState->AX != 0x004F) {
-		Log_Warning("VESA", "Vesa_Install - VESA/VBE Unsupported (AX = 0x%x)", gpVesa_BiosState->AX);
+		Log_Warning("VBE", "Vesa_Install - VESA/VBE Unsupported (AX = 0x%x)", gpVesa_BiosState->AX);
 		return MODULE_ERR_NOTNEEDED;
 	}
 	
@@ -117,49 +151,46 @@ int VBE_int_GetModeList(void)
 //	VM8086_Deallocate( gpVesa_BiosState, info );
 	
 	// Count Modes
-	for( giVesaModeCount = 0; modes[giVesaModeCount] != 0xFFFF; giVesaModeCount++ );
-	giVesaModeCount ++;	// First text mode
+	for( giVesaModeCount = 0; modes[giVesaModeCount] != 0xFFFF; giVesaModeCount++ )
+		;
 	gVesa_Modes = (tVesa_Mode *)malloc( giVesaModeCount * sizeof(tVesa_Mode) );
 	
 	Log_Debug("VBE", "%i Modes", giVesaModeCount);
 
 	// Insert Text Mode
-	gVesa_Modes[0].width = 80;
-	gVesa_Modes[0].height = 25;
-	gVesa_Modes[0].bpp = 12;
-	gVesa_Modes[0].code = 0x3;
-	gVesa_Modes[0].flags = 1;
-	gVesa_Modes[0].fbSize = 80*25*2;
-	gVesa_Modes[0].framebuffer = 0xB8000;
-	
-	for( i = 1; i < giVesaModeCount; i++ )
+	
+	for( int i = 0; i < giVesaModeCount; i++ )
 	{
-		gVesa_Modes[i].code = modes[i-1];
+		gVesa_Modes[i].code = modes[i];
 	}
 	
 	return 0;
 }
 
-void VBE_int_FillMode_Int(int Index, tVesa_CallModeInfo *vbeinfo, tFarPtr *BufPtr)
+int VBE_int_GetModeInfo(Uint16 Code, tFarPtr *BufPtr)
 {
-	tVesa_Mode	*mode = &gVesa_Modes[Index];
-
 	// Get Mode info
 	gpVesa_BiosState->AX = 0x4F01;
-	gpVesa_BiosState->CX = mode->code;
+	gpVesa_BiosState->CX = Code;
 	gpVesa_BiosState->ES = BufPtr->seg;
 	gpVesa_BiosState->DI = BufPtr->ofs;
 	VM8086_Int(gpVesa_BiosState, 0x10);
 
 	if( gpVesa_BiosState->AX != 0x004F ) {
-		Log_Error("VESA", "Getting info on mode 0x%x failed (AX=0x%x)",
-			mode->code, gpVesa_BiosState->AX);
-		return ;
+		Log_Error("VBE", "Getting info on mode 0x%x failed (AX=0x%x)",
+			Code, gpVesa_BiosState->AX);
+		return 1;
 	}
+	return 0;
+}
+#endif
 
-	#if 0
+
+void VBE_int_FillMode_Int(tVesa_Mode *Mode, const tVesa_CallModeInfo *vbeinfo)
+{
+	#if 1
 	#define S_LOG(s, fld, fmt)	LOG(" ."#fld" = "fmt, (s).fld)
-	LOG("vbeinfo[0x%x] = {", mode->code);
+	LOG("vbeinfo[0x%x] = {", Mode->code);
 	S_LOG(*vbeinfo, attributes, "0x%02x");
 	S_LOG(*vbeinfo, winA, "0x%02x");
 	S_LOG(*vbeinfo, winB, "0x%02x");
@@ -199,72 +230,121 @@ void VBE_int_FillMode_Int(int Index, tVesa_CallModeInfo *vbeinfo, tFarPtr *BufPt
 	S_LOG(*vbeinfo, image_count_lfb, "%i");
 	LOG("}");
 	#endif
-
-	mode->flags = FLAG_POPULATED;
-	if( !(vbeinfo->attributes & 1) ) {
+	
+	Mode->flags = FLAG_POPULATED;
+	// Check if this mode is supported by hardware
+	if( !(vbeinfo->attributes & 1) )
+	{
 		#if DEBUG
-		Log_Log("VESA", "0x%x - not supported", mode->code);
+		Log_Log("VBE", "0x%x - not supported", Mode->code);
 		#endif
-		mode->width = 0;
-		mode->height = 0;
-		mode->bpp = 0;
+		Mode->width = 0;
+		Mode->height = 0;
+		Mode->bpp = 0;
 		return ;
 	}
 
 	// Parse Info
-	mode->flags |= FLAG_VALID;
-	if ( (vbeinfo->attributes & 0x90) == 0x90 )
+	Mode->flags |= FLAG_VALID;
+	switch( vbeinfo->attributes & 0x90 )	// LFB, Graphics
 	{
-		mode->flags |= FLAG_LFB;
-		mode->framebuffer = vbeinfo->physbase;
-		mode->fbSize = vbeinfo->Yres*vbeinfo->pitch;
-	} else {
-		mode->framebuffer = 0;
-		mode->fbSize = 0;
+	case 0x00:	// Banked, Text
+	case 0x10:	// Banked, Graphics
+	case 0x80:	// Linear, Text (?)
+		Mode->width = 0;
+		Mode->height = 0;
+		Mode->bpp = 0;
+		return ;
+	case 0x90:
+		Mode->flags |= FLAG_LFB|FLAG_GRAPHICS;
+		Mode->framebuffer = vbeinfo->physbase;
+		Mode->fbSize = vbeinfo->Yres*vbeinfo->pitch;
+		break;
 	}
 	
-	mode->pitch = vbeinfo->pitch;
-	mode->width = vbeinfo->Xres;
-	mode->height = vbeinfo->Yres;
-	mode->bpp = vbeinfo->bpp;
+	Mode->pitch = vbeinfo->pitch;
+	Mode->width = vbeinfo->Xres;
+	Mode->height = vbeinfo->Yres;
+	Mode->bpp = vbeinfo->bpp;
 	
 	#if DEBUG
-	Log_Log("VESA", "#%i 0x%x - %ix%ix%i (%x)",
-		Index, mode->code, mode->width, mode->height, mode->bpp, mode->flags);
+	Log_Log("VBE", "0x%x - %ix%ix%i (%x)",
+		Mode->code, Mode->width, Mode->height, Mode->bpp, Mode->flags);
 	#endif
 
 } 
 
+void VBE_int_SetBootMode(Uint16 ModeID, const void *ModeInfo)
+{
+	gVesa_BootMode.code = ModeID;
+	VBE_int_FillMode_Int(&gVesa_BootMode, ModeInfo);
+}
+
 void Vesa_int_FillModeList(void)
 {
-	if( !gbVesaModesChecked )
+	#if USE_BIOS
+	if( !gbVesaModesChecked && !gbVesa_DisableBIOSCalls )
 	{
-		 int	i;
 		tVesa_CallModeInfo	*modeinfo;
 		tFarPtr	modeinfoPtr;
 		
 		modeinfo = VM8086_Allocate(gpVesa_BiosState, 256, &modeinfoPtr.seg, &modeinfoPtr.ofs);
-		for( i = 1; i < giVesaModeCount; i ++ )
+		for( int i = 0; i < giVesaModeCount; i ++ )
 		{
-			VBE_int_FillMode_Int(i, modeinfo, &modeinfoPtr);
+			if( VBE_int_GetModeInfo(gVesa_Modes[i].code, &modeinfoPtr) == 0 )
+			{
+				VBE_int_FillMode_Int( &gVesa_Modes[i], modeinfo );
+			}
 		}	
 //		VM8086_Deallocate( gpVesa_BiosState, modeinfo );
 		
 		gbVesaModesChecked = 1;
 	}
+	#endif
 }
 
 /**
  * \brief Write to the framebuffer
  */
-size_t Vesa_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer)
+size_t Vesa_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
 {
-	if( gVesa_Modes[giVesaCurrentMode].framebuffer == 0 ) {
-		Log_Warning("VESA", "Vesa_Write - Non-LFB Modes not yet supported.");
+	switch( gpVesaCurMode->flags & (FLAG_LFB|FLAG_GRAPHICS) )
+	{
+	case 0:
+		// EGA text mode translation
+		switch( gVesa_BufInfo.BufferFormat )
+		{
+		case VIDEO_BUFFMT_TEXT: {
+			 int	num = Length / sizeof(tVT_Char);
+			 int	ofs = Offset / sizeof(tVT_Char);
+			 int	i = 0;
+			const tVT_Char	*chars = Buffer;
+			
+			for( ; num--; i ++, ofs ++)
+			{
+				Uint16	word = VBE_int_GetWord( &chars[i] );
+				((Uint16*)gVesa_BufInfo.Framebuffer)[ ofs ] = word;
+			}
+			
+			return Length; }
+		case VIDEO_BUFFMT_2DSTREAM:
+			return DrvUtil_Video_2DStream(NULL, Buffer, Length,
+				&gVBE_Text2DFunctions, sizeof(gVBE_Text2DFunctions));
+		default:
+			Log_Warning("VBE", "TODO: Alternate modes in EGA text mode");
+			return 0;
+		}
+		return 0;
+	case FLAG_LFB|FLAG_GRAPHICS:
+		// Framebuffer modes - use DrvUtil Video
+		return DrvUtil_Video_WriteLFB(&gVesa_BufInfo, Offset, Length, Buffer);
+	default:
+		Log_Warning("VBE", "TODO: _Write %s%s",
+			(gpVesaCurMode->flags & FLAG_LFB ? "FLAG_LFB" : ""),
+			(gpVesaCurMode->flags & FLAG_GRAPHICS ? "FLAG_GRAPHICS" : "")
+			);
 		return 0;
 	}
-
-	return DrvUtil_Video_WriteLFB(&gVesa_BufInfo, Offset, Length, Buffer);
 }
 
 const char *csaVESA_IOCtls[] = {DRV_IOCTLNAMES, DRV_VIDEO_IOCTLNAMES, NULL};
@@ -276,7 +356,7 @@ int Vesa_IOCtl(tVFS_Node *Node, int ID, void *Data)
 	 int	ret;
 	switch(ID)
 	{
-	BASE_IOCTLS(DRV_TYPE_VIDEO, "VESA", VERSION, csaVESA_IOCtls);
+	BASE_IOCTLS(DRV_TYPE_VIDEO, "VBE", VERSION, csaVESA_IOCtls);
 
 	case VIDEO_IOCTL_GETSETMODE:
 		if( !Data )	return giVesaCurrentMode;
@@ -290,13 +370,16 @@ int Vesa_IOCtl(tVFS_Node *Node, int ID, void *Data)
 	case VIDEO_IOCTL_SETBUFFORMAT:
 		Vesa_int_HideCursor();
 		ret = gVesa_BufInfo.BufferFormat;
-		if(Data)	gVesa_BufInfo.BufferFormat = *(int*)Data;
+		if(Data)
+			gVesa_BufInfo.BufferFormat = *(int*)Data;
 		if(gVesa_BufInfo.BufferFormat == VIDEO_BUFFMT_TEXT)
-			DrvUtil_Video_SetCursor( &gVesa_BufInfo, &gDrvUtil_TextModeCursor );
+			Vesa_int_SetCursor(&gDrvUtil_TextModeCursor);
 		Vesa_int_ShowCursor();
 		return ret;
 	
 	case VIDEO_IOCTL_SETCURSOR:	// Set cursor position
+		if( !CheckMem(Data, sizeof(tVideo_IOCtl_Pos)) )
+			return -EINVAL;
 		Vesa_int_HideCursor();
 		giVesaCursorX = ((tVideo_IOCtl_Pos*)Data)->x;
 		giVesaCursorY = ((tVideo_IOCtl_Pos*)Data)->y;
@@ -304,8 +387,7 @@ int Vesa_IOCtl(tVFS_Node *Node, int ID, void *Data)
 		return 0;
 	
 	case VIDEO_IOCTL_SETCURSORBITMAP:
-		DrvUtil_Video_SetCursor( &gVesa_BufInfo, Data );
-		return 0;
+		return Vesa_int_SetCursor(Data);
 	}
 	return 0;
 }
@@ -314,163 +396,240 @@ int Vesa_IOCtl(tVFS_Node *Node, int ID, void *Data)
  * \brief Updates the video mode
  */
 int Vesa_Int_SetMode(int mode)
-{	
-	// Sanity Check values
-	if(mode < 0 || mode > giVesaModeCount)	return -1;
-
+{
+	tVesa_Mode	*modeptr;
 	// Check for fast return
 	if(mode == giVesaCurrentMode)	return 1;
 	
+	// Special case: Boot mode
+	if( mode == -1 )
+		modeptr = &gVesa_BootMode;
+	else if( 0 <= mode && mode < giVesaModeCount )
+		modeptr = &gVesa_Modes[mode];
+	else
+		return -1;
+	
 	Vesa_int_FillModeList();
 
+	#if BLINKING_CURSOR
 	Time_RemoveTimer(gpVesaCursorTimer);
+	#endif
 	
 	Mutex_Acquire( &glVesa_Lock );
-	
-	gpVesa_BiosState->AX = 0x4F02;
-	gpVesa_BiosState->BX = gVesa_Modes[mode].code;
-	if(gVesa_Modes[mode].flags & FLAG_LFB) {
-		gpVesa_BiosState->BX |= 1 << 14;	// Use LFB
+
+	#if USE_BIOS
+	if( gbVesa_DisableBIOSCalls )
+	{
+		ASSERT(mode == -1);
 	}
-	LOG("In : AX=%04x/BX=%04x",
-		gpVesa_BiosState->AX, gpVesa_BiosState->BX);
-	
-	// Set Mode
-	VM8086_Int(gpVesa_BiosState, 0x10);
+	else
+	{
+		gpVesa_BiosState->AX = 0x4F02;
+		gpVesa_BiosState->BX = modeptr->code;
+		if(modeptr->flags & FLAG_LFB) {
+			gpVesa_BiosState->BX |= 1 << 14;	// Use LFB
+		}
+		LOG("In : AX=%04x/BX=%04x", gpVesa_BiosState->AX, gpVesa_BiosState->BX);
+		
+		// Set Mode
+		VM8086_Int(gpVesa_BiosState, 0x10);
 
-	LOG("Out: AX = %04x", gpVesa_BiosState->AX);
+		LOG("Out: AX = %04x", gpVesa_BiosState->AX);
+	}
+	#else
+	ASSERT(mode == -1);
+	#endif
 	
 	// Map Framebuffer
-	if( (tVAddr)gpVesa_Framebuffer != VESA_DEFAULT_FRAMEBUFFER )
-		MM_UnmapHWPages((tVAddr)gpVesa_Framebuffer, giVesaPageCount);
-	giVesaPageCount = (gVesa_Modes[mode].fbSize + 0xFFF) >> 12;
-	gpVesa_Framebuffer = (void*)MM_MapHWPages(gVesa_Modes[mode].framebuffer, giVesaPageCount);
-	
-	Log_Log("VESA", "Setting mode to %i 0x%x (%ix%i %ibpp) %p[0x%x] maps %P",
-		mode, gVesa_Modes[mode].code,
-		gVesa_Modes[mode].width, gVesa_Modes[mode].height,
-		gVesa_Modes[mode].bpp,
-		gpVesa_Framebuffer, giVesaPageCount << 12, gVesa_Modes[mode].framebuffer
+	if( gpVesaCurMode )
+	{
+		MM_UnmapHWPages(gpVesa_Framebuffer, giVesaPageCount);
+	}
+	giVesaPageCount = (modeptr->fbSize + 0xFFF) >> 12;
+	gpVesa_Framebuffer = MM_MapHWPages(modeptr->framebuffer, giVesaPageCount);
+	
+	Log_Log("VBE", "Setting mode to %i 0x%x (%ix%i %ibpp) %p[0x%x] maps %P",
+		mode, modeptr->code,
+		modeptr->width, modeptr->height,
+		modeptr->bpp,
+		gpVesa_Framebuffer, giVesaPageCount << 12, modeptr->framebuffer
 		);
 	
 	// Record Mode Set
 	giVesaCurrentMode = mode;
-	gpVesaCurMode = &gVesa_Modes[giVesaCurrentMode];
+	gpVesaCurMode = modeptr;
 	
 	Mutex_Release( &glVesa_Lock );
 
+	// TODO: Allow disabling of back-buffer
+	gVesa_DriverStruct.RootNode.Size = modeptr->height * modeptr->pitch;
+	gVesa_BufInfo.BackBuffer  = realloc(gVesa_BufInfo.BackBuffer, modeptr->height * modeptr->pitch);
 	gVesa_BufInfo.Framebuffer = gpVesa_Framebuffer;
-	gVesa_BufInfo.Pitch = gVesa_Modes[mode].pitch;
-	gVesa_BufInfo.Width = gVesa_Modes[mode].width;
-	gVesa_BufInfo.Height = gVesa_Modes[mode].height;
-	gVesa_BufInfo.Depth = gVesa_Modes[mode].bpp;	
+	gVesa_BufInfo.Pitch = modeptr->pitch;
+	gVesa_BufInfo.Width = modeptr->width;
+	gVesa_BufInfo.Height = modeptr->height;
+	gVesa_BufInfo.Depth = modeptr->bpp;	
 
 	return 1;
 }
 
-int Vesa_Int_FindMode(tVideo_IOCtl_Mode *data)
+int VBE_int_MatchModes(tVideo_IOCtl_Mode *ReqMode, tVesa_Mode *ThisMode)
 {
-	 int	i;
-	 int	best = -1, tmp;
-	unsigned int factor, bestFactor = -1;
-	
-	ENTER("idata->width idata->height idata->bpp", data->width, data->height, data->bpp);
-
-	Vesa_int_FillModeList();
-	
-	for(i = 0; i < giVesaModeCount; i ++)
+	if( ThisMode->bpp == 0 ) {
+		Log_Warning("VBE", "VESA mode %x (%ix%i) has 0bpp",
+			ThisMode->code, ThisMode->width, ThisMode->height);
+		return INT_MAX;
+	}
+	LOG("Matching %ix%i %ibpp", ThisMode->width, ThisMode->height, ThisMode->bpp);
+	if(ThisMode->width == ReqMode->width && ThisMode->height == ReqMode->height)
 	{
-		LOG("Mode %i (%ix%ix%i)", i, gVesa_Modes[i].width, gVesa_Modes[i].height, gVesa_Modes[i].bpp);
-	
-		if(gVesa_Modes[i].width == data->width && gVesa_Modes[i].height == data->height)
+		//if( (data->bpp == 32 || data->bpp == 24)
+		// && (gVesa_Modes[i].bpp == 32 || gVesa_Modes[i].bpp == 24) )
+		if( ReqMode->bpp == ThisMode->bpp )
 		{
-			//if( (data->bpp == 32 || data->bpp == 24)
-			// && (gVesa_Modes[i].bpp == 32 || gVesa_Modes[i].bpp == 24) )
-			if( data->bpp == gVesa_Modes[i].bpp )
-			{
-				LOG("Perfect!");
-				best = i;
-				break;
-			}
+			LOG("Perfect!");
+			return -1;
 		}
+	}
+	
+	int tmp = ThisMode->width * ThisMode->height - ReqMode->width * ReqMode->height;
+	tmp = tmp < 0 ? -tmp : tmp;
+	unsigned int factor = (Uint64)tmp * 1000 / (ReqMode->width * ReqMode->height);
+	if( ThisMode->bpp > ReqMode->bpp )
+		factor += ThisMode->bpp - ReqMode->bpp;
+	else
+		factor += ReqMode->bpp - ThisMode->bpp;
+
+	if( ReqMode->bpp == ThisMode->bpp )
+		factor /= 2;
+	else
+	{
+		if( ReqMode->bpp == 8 && ThisMode->bpp != 8 )	factor *= 4;
+		if( ReqMode->bpp == 16 && ThisMode->bpp != 16 )	factor *= 4;
 		
-		tmp = gVesa_Modes[i].width * gVesa_Modes[i].height;
-		tmp -= data->width * data->height;
-		tmp = tmp < 0 ? -tmp : tmp;
-		factor = (Uint64)tmp * 1000 / (data->width * data->height);
-		
-		if( data->bpp == 8 && gVesa_Modes[i].bpp != 8 )	continue;
-		if( data->bpp == 16 && gVesa_Modes[i].bpp != 16 )	continue;
-		
-		if( (data->bpp == 32 || data->bpp == 24)
-		 && (gVesa_Modes[i].bpp == 32 || gVesa_Modes[i].bpp == 24) )
+		if( (ReqMode->bpp == 32 || ReqMode->bpp == 24)
+		 && (ThisMode->bpp == 32 || ThisMode->bpp == 24) )
 		{
-			if( data->bpp == gVesa_Modes[i].bpp )
-				factor /= 2;
+			// NC
 		}
 		else {
-			if( data->bpp != gVesa_Modes[i].bpp )
-				continue ;
+			if( ReqMode->bpp < ThisMode->bpp )
+				factor *= ThisMode->bpp / ReqMode->bpp + 1;
+			else
+				factor *= ReqMode->bpp / ThisMode->bpp + 1;
 		}
+	}
+	
+	return factor;
+}
+
+int Vesa_Int_FindMode(tVideo_IOCtl_Mode *data)
+{
+	 int	best = -1;
+	
+	ENTER("idata->width idata->height idata->bpp", data->width, data->height, data->bpp);
+
+	Vesa_int_FillModeList();
+
+	int bestFactor = VBE_int_MatchModes(data, &gVesa_BootMode);	
+	tVesa_Mode *bestPtr = &gVesa_BootMode;
+
+	for(int i = 0; bestFactor > 0 && i < giVesaModeCount; i ++)
+	{
+		LOG("Mode %i (%ix%ix%i)", i, gVesa_Modes[i].width, gVesa_Modes[i].height, gVesa_Modes[i].bpp);
+	
+		int factor = VBE_int_MatchModes(data, &gVesa_Modes[i]);
 		
-		LOG("factor = %i", factor);
+		LOG("factor = %i, bestFactor = %i", factor, bestFactor);
 		
 		if(factor < bestFactor)
 		{
 			bestFactor = factor;
 			best = i;
+			bestPtr = &gVesa_Modes[i];
 		}
 	}
 	data->id = best;
-	data->width = gVesa_Modes[best].width;
-	data->height = gVesa_Modes[best].height;
-	data->bpp = gVesa_Modes[best].bpp;
+	data->width = bestPtr->width;
+	data->height = bestPtr->height;
+	data->bpp = bestPtr->bpp;
 	LEAVE('i', best);
 	return best;
 }
 
 int Vesa_Int_ModeInfo(tVideo_IOCtl_Mode *data)
 {
-	if(data->id < 0 || data->id > giVesaModeCount)	return -1;
+	tVesa_Mode	*modeptr;
+	if( data->id == -1 )
+		modeptr = &gVesa_BootMode;
+	else if( 0 <= data->id && data->id < giVesaModeCount)
+		modeptr = &gVesa_Modes[data->id];
+	else
+		return 0;
 
 	Vesa_int_FillModeList();
 
-	data->width = gVesa_Modes[data->id].width;
-	data->height = gVesa_Modes[data->id].height;
-	data->bpp = gVesa_Modes[data->id].bpp;
+	data->width  = modeptr->width;
+	data->height = modeptr->height;
+	data->bpp    = modeptr->bpp;
 	return 1;
 }
 
 void Vesa_int_HideCursor(void)
 {
 	DrvUtil_Video_RemoveCursor( &gVesa_BufInfo );
-	#if BLINKING_CURSOR
-	if(gpVesaCursorTimer) {
-		Time_RemoveTimer(gpVesaCursorTimer);
+	if( gpVesaCurMode->flags & FLAG_LFB )
+	{
+		#if BLINKING_CURSOR
+		if(gpVesaCursorTimer) {
+			Time_RemoveTimer(gpVesaCursorTimer);
+		}
+		#endif
 	}
-	#endif
 }
 
 void Vesa_int_ShowCursor(void)
 {
-	gbVesa_CursorVisible = (giVesaCursorX >= 0);
-	if(gVesa_BufInfo.BufferFormat == VIDEO_BUFFMT_TEXT)
+	if( gpVesaCurMode && gpVesaCurMode->flags & FLAG_LFB )
 	{
-		DrvUtil_Video_DrawCursor(
-			&gVesa_BufInfo,
-			giVesaCursorX*giVT_CharWidth,
-			giVesaCursorY*giVT_CharHeight
-			);
-		#if BLINKING_CURSOR
-		Time_ScheduleTimer( gpVesaCursorTimer, VESA_CURSOR_PERIOD );
-		#endif
+		gbVesa_CursorVisible = (giVesaCursorX >= 0);
+		if(gVesa_BufInfo.BufferFormat == VIDEO_BUFFMT_TEXT)
+		{
+			DrvUtil_Video_DrawCursor(
+				&gVesa_BufInfo,
+				giVesaCursorX*giVT_CharWidth,
+				giVesaCursorY*giVT_CharHeight
+				);
+			#if BLINKING_CURSOR
+			Time_ScheduleTimer( gpVesaCursorTimer, VESA_CURSOR_PERIOD );
+			#endif
+		}
+		else
+			DrvUtil_Video_DrawCursor(
+				&gVesa_BufInfo,
+				giVesaCursorX,
+				giVesaCursorY
+				);
 	}
 	else
-		DrvUtil_Video_DrawCursor(
-			&gVesa_BufInfo,
-			giVesaCursorX,
-			giVesaCursorY
-			);
+	{
+		DrvUtil_Video_RemoveCursor( &gVesa_BufInfo );
+	}
+}
+
+int Vesa_int_SetCursor(tVideo_IOCtl_Bitmap *Cursor)
+{
+	if( !CheckMem(Cursor, sizeof(tVideo_IOCtl_Bitmap)) )
+		return -EINVAL;
+	
+	if( gpVesaCurMode && gpVesaCurMode->flags & FLAG_LFB )
+	{
+		DrvUtil_Video_SetCursor( &gVesa_BufInfo, Cursor );
+	}
+	else
+	{
+	}
+	return 0;
 }
 
 /**
@@ -481,17 +640,182 @@ void Vesa_FlipCursor(void *Arg)
 	if( gVesa_BufInfo.BufferFormat != VIDEO_BUFFMT_TEXT )
 		return ;
 
-	if( gbVesa_CursorVisible )
-		DrvUtil_Video_RemoveCursor(&gVesa_BufInfo);
+	if( gpVesaCurMode && gpVesaCurMode->flags & FLAG_LFB )
+	{
+		if( gbVesa_CursorVisible )
+			DrvUtil_Video_RemoveCursor(&gVesa_BufInfo);
+		else
+			DrvUtil_Video_DrawCursor(&gVesa_BufInfo,
+				giVesaCursorX*giVT_CharWidth,
+				giVesaCursorY*giVT_CharHeight
+				);
+		gbVesa_CursorVisible = !gbVesa_CursorVisible;
+			
+		#if BLINKING_CURSOR
+		Time_ScheduleTimer( gpVesaCursorTimer, VESA_CURSOR_PERIOD );
+		#endif
+	}
+}
+
+// ---
+// Helpers for text mode
+// ---
+
+/**
+ * \fn Uint8 VGA_int_GetColourNibble(Uint16 col)
+ * \brief Converts a 12-bit colour into a VGA 4-bit colour
+ */
+Uint8 VBE_int_GetColourNibble(Uint16 col)
+{
+	Uint8	ret = 0;
+	 int	bright = 0;
+	
+	col = col & 0xCCC;
+	col = ((col>>2)&3) | ((col>>4)&0xC) | ((col>>6)&0x30);
+	bright = ( (col & 2 ? 1 : 0) + (col & 8 ? 1 : 0) + (col & 32 ? 1 : 0) ) / 2;
+	
+	switch(col)
+	{
+	//	Black
+	case 0x00:	ret = 0x0;	break;
+	// Dark Grey
+	case 0x15:	ret = 0x8;	break;
+	// Blues
+	case 0x01:
+	case 0x02:	ret = 0x1;	break;
+	case 0x03:	ret = 0x9;	break;
+	// Green
+	case 0x04:
+	case 0x08:	ret = 0x2;	break;
+	case 0x0C:	ret = 0xA;	break;
+	// Reds
+	case 0x10:
+	case 0x20:	ret = 0x4;	break;
+	case 0x30:	ret = 0xC;	break;
+	// Light Grey
+	case 0x2A:	ret = 0x7;	break;
+	// White
+	case 0x3F:	ret = 0xF;	break;
+	
+	default:
+		ret |= (col & 0x03 ? 1 : 0);
+		ret |= (col & 0x0C ? 2 : 0);
+		ret |= (col & 0x30 ? 4 : 0);
+		ret |= (bright ? 8 : 0);
+		break;
+	}
+	return ret;
+}
+
+/**
+ * \brief Convers a character structure to a VGA character word
+ */
+Uint16 VBE_int_GetWord(const tVT_Char *Char)
+{
+	Uint16	ret;
+	Uint16	col;
+	
+	// Get Character
+	if(Char->Ch < 128)
+		ret = Char->Ch;
+	else {
+		switch(Char->Ch)
+		{
+		default:	ret = 0;	break;
+		}
+	}
+	
+	col = VBE_int_GetColourNibble(Char->BGCol);
+	ret |= col << 12;
+	
+	col = VBE_int_GetColourNibble(Char->FGCol);
+	ret |= col << 8;
+	
+	return ret;
+}
+
+void VBE_int_Text_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour)
+{
+	const int	charw = 8;
+	const int	charh = 16;
+	const int	tw = gpVesaCurMode->width / charw;
+	const int	th = gpVesaCurMode->height / charh;
+
+	X /= charw;
+	W /= charw;
+	Y /= charh;
+	H /= charh;
+
+	tVT_Char	ch;
+	ch.Ch = 0x20;
+	ch.BGCol  = (Colour & 0x0F0000) >> (16-8);
+	ch.BGCol |= (Colour & 0x000F00) >> (8-4);
+	ch.BGCol |= (Colour & 0x00000F);
+	ch.FGCol = 0;
+	Uint16 word = VBE_int_GetWord(&ch);
+
+	Log("Fill (%i,%i) %ix%i with 0x%x", X, Y, W, H, word);
+
+	if( X >= tw || Y >= th )	return ;
+	if( X + W > tw )	W = tw - X;
+	if( Y + H > th )	H = th - Y;
+
+	Uint16	*buf = (Uint16*)gpVesa_Framebuffer + Y*tw + X;
+
+	
+	while( H -- ) {
+		for( int i = 0; i < W; i ++ )
+			*buf++ = word;
+		buf += tw - W;
+	}
+}
+
+void VBE_int_Text_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H)
+{
+	const int	charw = 8;
+	const int	charh = 16;
+	const int	tw = gpVesaCurMode->width / charw;
+	const int	th = gpVesaCurMode->height / charh;
+
+	DstX /= charw;
+	SrcX /= charw;
+	W    /= charw;
+
+	DstY /= charh;
+	SrcY /= charh;
+	H    /= charh;
+
+//	Log("(%i,%i) from (%i,%i) %ix%i", DstX, DstY, SrcX, SrcY, W, H);
+
+	if( SrcX >= tw || SrcY >= th )	return ;
+	if( SrcX + W > tw )	W = tw - SrcX;
+	if( SrcY + H > th )	H = th - SrcY;
+	if( DstX >= tw || DstY >= th )	return ;
+	if( DstX + W > tw )	W = tw - DstX;
+	if( DstY + H > th )	H = th - DstY;
+
+
+	Uint16	*src = (Uint16*)gpVesa_Framebuffer + SrcY*tw + SrcX;
+	Uint16	*dst = (Uint16*)gpVesa_Framebuffer + DstY*tw + DstX;
+
+	if( src > dst )
+	{
+		// Simple copy
+		while( H-- ) {
+			memcpy(dst, src, W*2);
+			dst += tw;
+			src += tw;
+		}
+	}
 	else
-		DrvUtil_Video_DrawCursor(&gVesa_BufInfo,
-			giVesaCursorX*giVT_CharWidth,
-			giVesaCursorY*giVT_CharHeight
-			);
-	gbVesa_CursorVisible = !gbVesa_CursorVisible;
-		
-	#if BLINKING_CURSOR
-	Time_ScheduleTimer( gpVesaCursorTimer, VESA_CURSOR_PERIOD );
-	#endif
+	{
+		dst += H*tw;
+		src += H*tw;
+		while( H -- ) {
+			dst -= tw-W;
+			src -= tw-W;
+			for( int i = W; i --; )	*--dst = *--src;
+		}
+	}
 }