VGA Graphics emulation?

[superfury]

I'm currently working on VGA Graphics emulation (the rendering part. Text emulation already works). For some reason I keep getting blue pixels everywhere, when trying to use 256 or 16 color modes?

byte logpixels = 0;

byte VGA_getpixel(VGA_Type *VGA, int x, int y)
{
	word vramstart = ((VGA->registers->CRTControllerRegisters.REGISTERS.STARTADDRESSHIGHREGISTER<<8)|VGA->registers->CRTControllerRegisters.REGISTERS.STARTADDRESSLOWREGISTER); //Start address of VRAM display!
	//vramstart += getVRAMScanlineStart(VGA,y)*getVRAMScanlineMultiplier(VGA); //Start of the graphics row!
	
	uint_32 offset; //The offset!
	byte bit; //The bit we need!

	if (VGA->registers->GraphicsRegisters.REGISTERS.GRAPHICSMODEREGISTER.Color256ShiftMode) //256-color mode?
	{
		/*uint_32 planeoffset = getVRAMScanlineStart(VGA,y)+x; //The full offset!
		byte result = readVRAMplane(VGA,vramstart,planeoffset%4,planeoffset/4); //Get the 8-bit pixel!
		*/
		offset = getVRAMScanlineStart(VGA,y)+x; //The offset!

		if (logpixels)
		{
			dolog("VGA_VRAM","Scanline size: %i",getVRAMScanlineStart(VGA,1));
			dolog("VGA_VRAM","VRAM Start: %i",vramstart);
			logpixels = 0; //No logging anymore!
		}
		/*if (logpixels && y<5) //First 5 rows only?
		{
			dolog("VRAM","%03i,%03i=%05X",x,y,offset); //Log the result!
			if (x==getxres(VGA) && y==5) //Last X?
			{
				++logpixels; //Next pixel level!
			}
			if (logpixels>2) //More than 2 screens?
			{
				logpixels = 0; //Not logpixels anymore!
			}
		} //Should be OK!
		*/
		return readVRAMdirect(VGA,vramstart,offset); //Seems to work...
	}
	else if (VGA->registers->GraphicsRegisters.REGISTERS.GRAPHICSMODEREGISTER.ShiftRegisterInterleaveMode) //Shift Register Interleave mode?
	{
		uint_32 planeindex = /*getVRAMScanlineStart(VGA,y)+ */ (x/8); //Plane index, should be OK for row 0!
		byte planelow = readVRAMplane(VGA,vramstart,(x%8)/4,planeindex); //Lower plane: OK!
		byte planehigh = readVRAMplane(VGA,vramstart,((x%8)/4)+2,planeindex); //Higher plane: OK!
		byte shift = 6-((x%8)%4)*2; //OK!
		byte bitmask = 3<<shift; //The bitmask for the lower&higher planes: OK!
		return ( //This should be OK!
			((planelow&bitmask)>>shift)|
			(((planehigh&bitmask)>>shift)<<2)
			); //Give the VRAM value of the specified pixel!
	}
	else if (!VGA->registers->ExternalRegisters.MISCOUTPUTREGISTER.IO_AS) //Monochrome mode?
	{
		offset = (getVRAMScanlineStart(VGA,y)+((x/8)*getVRAMMemAddrSize(VGA))); //Offset!
		bit = 7-(x%8); //The bit in the byte!
		return getBitPlaneBit(VGA,vramstart,0,offset,bit); //Give the bit from VRAM plane 0!
	}
	else //16-color mode = Single shift?
	{
		//TODO!
		return 0xF; //White out!
		bit = 7-((x%4)*2); //The bit number!
		offset = (getVRAMScanlineStart(VGA,y)+((x/8)*getVRAMMemAddrSize(VGA)));
		return (getBitPlaneBit(VGA,vramstart,0,offset,bit)| //Bit0
			(getBitPlaneBit(VGA,vramstart,1,offset,bit)<<1)| //Bit1
			(getBitPlaneBit(VGA,vramstart,2,offset,bit)<<2)| //Bit2
			(getBitPlaneBit(VGA,vramstart,3,offset,bit)<<3)); //Bit3
	}
}

Btw, getBitPlaneBit(VGA instance(Which holds the full VRAM and all other VGA related settings, registers etc.), VRAM start address(0-VRAMsize), Plane number(0-3), Byte number within Plane(Normally 0-65535), Bit(0=9)) gives a bit from VRAM.
readVRAMplane is the same, but without the bit option (thus returning a whole byte of VRAM).
getVRAMScanLineStart(VGA,Scanline) returns the start address of a scanline in VRAM, from the beginning of VRAM (Using a precalculated value:

uint_32 getVRAMScanlineStart(VGA_Type *VGA,word Scanline) //Start of a scanline!
{
	return VGA->precalcs.scanlinesize*Scanline; //Give the start of the row!
}

Calculation of precalcs scanlinesize:

VGA->precalcs.rowsize = (VGA->registers->CRTControllerRegisters.REGISTERS.OFFSETREGISTER<<1);

...

VGA->precalcs.VRAMmemaddrsize = VGA->registers->CRTControllerRegisters.REGISTERS.UNDERLINELOCATIONREGISTER.DW?4:
							(VGA->registers->CRTControllerRegisters.REGISTERS.CRTCMODECONTROLREGISTER.UseByteMode?1:2); //Memory address size!

...

VGA->precalcs.scanlinesize = VGA->precalcs.rowsize*VGA->precalcs.VRAMmemaddrsize; //Scanline size! 

)

[superfury]

Got it working so far on the 256 color shift mode and the Monochrome mode. Anyone knows what's going wrong with the Shift Register Interleave mode and the 16-color mode?