#include "vgaHW.h"
#include "smi.h"

/*
 *  Settings of standard VGA registers.
 */
static void SMI_vgaHWRestoreMode(SMIPtr pSmi, SMI_vgaRegPtr restore)
{
	int i;

	for (i = 1; i < restore->numSequencer; i++)
		VGAOUT8_INDEX(pSmi, VGA_SEQ_INDEX, VGA_SEQ_DATA, i, restore->Sequencer[i]); 

	/* Ensure CRTC registers 0-7 are unlocked by clearing bit 7 of CRTC[17] */
	VGAOUT8_INDEX(pSmi, VGA_IOBASE_COLOR + VGA_CRTC_INDEX_OFFSET, 
			VGA_IOBASE_COLOR + VGA_CRTC_DATA_OFFSET, 17, restore->CRTC[17] & ~0x80);

	for (i = 0; i < restore->numCRTC; i++)
		VGAOUT8_INDEX(pSmi, VGA_IOBASE_COLOR + VGA_CRTC_INDEX_OFFSET, 
				VGA_IOBASE_COLOR + VGA_CRTC_DATA_OFFSET, i, restore->CRTC[i]);

	for (i = 0; i < restore->numGraphics; i++)
		VGAOUT8_INDEX(pSmi, VGA_GRAPH_INDEX, VGA_GRAPH_DATA, i, restore->Graphics[i]); 

	/* enable palette */
	VGAIN8(pSmi, VGA_IOBASE_COLOR + VGA_IN_STAT_1_OFFSET);
	VGAOUT8(pSmi, VGA_ATTR_INDEX, 0x00);

	for (i = 0; i < restore->numAttribute; i++)
		VGAOUT8_INDEX(pSmi, VGA_ATTR_INDEX, VGA_ATTR_DATA_W, i & ~0x20, restore->Attribute[i]);

	/* disable palette */
	VGAIN8(pSmi, VGA_IOBASE_COLOR + VGA_IN_STAT_1_OFFSET);
	VGAOUT8(pSmi, VGA_ATTR_INDEX, 0x20);
}

static void SMI_vgaHWRestoreFonts(SMIPtr pSmi, SMI_vgaRegPtr restore)
{

}


static void SMI_vgaHWRestoreColormap(SMIPtr pSmi, SMI_vgaRegPtr restore)
{
	int i;

	VGAOUT8(pSmi, VGA_DAC_MASK, 0xff);
	VGAOUT8(pSmi, VGA_DAC_READ_ADDR, 0x00);

	for (i = 0; i < sizeof(restore->DAC); i++) {
		VGAOUT8(pSmi, VGA_DAC_DATA, restore->DAC[i]);
	}

	VGAIN8(pSmi, VGA_IOBASE_COLOR + VGA_IN_STAT_1_OFFSET);
	VGAOUT8(pSmi, VGA_ATTR_INDEX, 0x20);
}

void SMI_vgaHWRestore(SMIPtr pSmi, SMI_vgaRegPtr restore, int flags)
{
        if (flags & VGA_SR_MODE)
                  SMI_vgaHWRestoreMode(pSmi, restore);

        if (flags & VGA_SR_FONTS)
                  SMI_vgaHWRestoreFonts(pSmi, restore);

        if (flags & VGA_SR_CMAP)
                  SMI_vgaHWRestoreColormap(pSmi, restore);
}


static void SMI_vgaHWSaveColormap(SMIPtr pSmi, SMI_vgaRegPtr save)
{
	int i;

	ENTER_PROC("SMI_vgaHWSaveColormap");

	VGAOUT8(pSmi, VGA_DAC_MASK, 0xff);
	VGAOUT8(pSmi, VGA_DAC_READ_ADDR, 0x00);
	for (i = 0; i < sizeof(save->DAC); i++) {
		save->DAC[i] = VGAIN8(pSmi, VGA_DAC_DATA);
	}

	VGAIN8(pSmi, VGA_IOBASE_COLOR + VGA_IN_STAT_1_OFFSET);
	VGAOUT8(pSmi, VGA_ATTR_INDEX, 0x20);
}

static void SMI_vgaHWSaveMode(SMIPtr pSmi, SMI_vgaRegPtr save)
{
	int i;

	ENTER_PROC("SMI_vgaHWSaveMode");

	for (i = 0; i < save->numCRTC; i++) {
		save->CRTC[i] = VGAIN8_INDEX(pSmi, VGA_IOBASE_COLOR + VGA_CRTC_INDEX_OFFSET, 
				VGA_IOBASE_COLOR + VGA_CRTC_DATA_OFFSET, i);
	}

	/* enable palette */
	VGAIN8(pSmi, VGA_IOBASE_COLOR + VGA_IN_STAT_1_OFFSET);
	VGAOUT8(pSmi, VGA_ATTR_INDEX, 0x00);

	for (i = 0; i < save->numAttribute; i++) {
		save->Attribute[i] = VGAIN8_INDEX(pSmi, VGA_ATTR_INDEX, VGA_ATTR_DATA_R, i & ~0x20);
	}

	/* disable palette */
	VGAIN8(pSmi, VGA_IOBASE_COLOR + VGA_IN_STAT_1_OFFSET);
	VGAOUT8(pSmi, VGA_ATTR_INDEX, 0x20);

	for (i = 0; i < save->numGraphics; i++) {
		save->Graphics[i] = VGAIN8_INDEX(pSmi, VGA_GRAPH_INDEX, VGA_GRAPH_DATA, i); 
	}

	for (i = 1; i < save->numSequencer; i++) {
		save->Graphics[i] = VGAIN8_INDEX(pSmi, VGA_SEQ_INDEX, VGA_SEQ_DATA, i); 
	}
}


static void SMI_vgaHWSaveFonts(SMIPtr pSmi, SMI_vgaRegPtr save)
{
	
}

void SMI_vgaHWSave (SMIPtr pSmi, SMI_vgaRegPtr save, int flags)
{
	if (save == NULL)
		return;

	ENTER_PROC("SMI_vgaHWSave");
	if (flags & VGA_SR_CMAP)
		SMI_vgaHWSaveColormap(pSmi, save);

	if (flags & VGA_SR_MODE)
		SMI_vgaHWSaveMode(pSmi, save);

	if (flags & VGA_SR_FONTS)
		SMI_vgaHWSaveFonts(pSmi, save);
}