/*
	Haiku S3 Savage driver adapted from the X.org Savage driver.

	Copyright (C) 1994-2000 The XFree86 Project, Inc.	All Rights Reserved.
	Copyright (c) 2003-2006, X.Org Foundation

	Copyright 2007-2008 Haiku, Inc.  All rights reserved.
	Distributed under the terms of the MIT license.

	Authors:
	Gerald Zajac 2006-2008
*/


#include "accel.h"
#include "savage.h"


#define BASE_FREQ	14.31818


struct SavageRegRec {
	uint8  CRTC[25];			// Crtc Controller reg's

	uint8  SR12, SR13, SR1B, SR29;
	uint8  CR33, CR34, CR3A, CR3B, CR3C;
	uint8  CR42, CR43, CR45;
	uint8  CR50, CR51, CR53, CR58, CR5D, CR5E;
	uint8  CR65, CR66, CR67, CR69;
	uint8  CR86, CR88;
	uint8  CR90, CR91, CRB0;
};



static void 
Savage_SetGBD_Twister(const DisplayModeEx& mode)
{
	SharedInfo& si = *gInfo.sharedInfo;
	int bci_enable;

	if (si.chipType == S3_SAVAGE4)
		bci_enable = BCI_ENABLE;
	else
		bci_enable = BCI_ENABLE_TWISTER;

	// MM81C0 and 81C4 are used to control primary stream.
	WriteReg32(PRI_STREAM_FBUF_ADDR0, 0);
	WriteReg32(PRI_STREAM_FBUF_ADDR1, 0);

	// Program Primary Stream Stride Register.
	//
	// Tell engine if tiling on or off, set primary stream stride, and
	// if tiling, set tiling bits/pixel and primary stream tile offset.
	// Note that tile offset (bits 16 - 29) must be scanline width in
	// bytes/128bytespertile * 256 Qwords/tile.  This is equivalent to
	// lDelta * 2.  Remember that if tiling, lDelta is screenwidth in
	// bytes padded up to an even number of tilewidths.

	WriteReg32(PRI_STREAM_STRIDE,
		(((mode.bytesPerRow * 2) << 16) & 0x3FFFE000) |
		(mode.bytesPerRow & 0x00001fff));

	//  CR69, bit 7 = 1
	//  to use MM streams processor registers to control primary stream.

	WriteCrtcReg(0x69, 0x80, 0x80);

	WriteReg32(S3_GLOBAL_GBD_REG, bci_enable | S3_LITTLE_ENDIAN | S3_BD64);

	// If MS1NB style linear tiling mode.
	// bit MM850C[15] = 0 select NB linear tile mode.
	// bit MM850C[15] = 1 select MS-1 128-bit non-linear tile mode.

	uint32 ulTmp = ReadReg32(ADVANCED_FUNC_CTRL) | 0x8000;	// use MS-s style tile mode
	WriteReg32(ADVANCED_FUNC_CTRL, ulTmp);

	// CR88, bit 4 - Block write enabled/disabled.
	//
	// Note: Block write must be disabled when writing to tiled
	//		 memory. Even when writing to non-tiled memory, block
	//		 write should only be enabled for certain types of SGRAM.

	WriteCrtcReg(0x88, DISABLE_BLOCK_WRITE_2D, DISABLE_BLOCK_WRITE_2D);
}


static void 
Savage_SetGBD_3D(const DisplayModeEx& mode)
{
	int bci_enable = BCI_ENABLE;

	// MM81C0 and 81C4 are used to control primary stream.
	WriteReg32(PRI_STREAM_FBUF_ADDR0, 0);
	WriteReg32(PRI_STREAM_FBUF_ADDR1, 0);

	// Program Primary Stream Stride Register.
	//
	// Tell engine if tiling on or off, set primary stream stride, and
	// if tiling, set tiling bits/pixel and primary stream tile offset.
	// Note that tile offset (bits 16 - 29) must be scanline width in
	// bytes/128bytespertile * 256 Qwords/tile.  This is equivalent to
	// lDelta * 2.  Remember that if tiling, lDelta is screenwidth in
	// bytes padded up to an even number of tilewidths.

	WriteReg32(PRI_STREAM_STRIDE,
		(((mode.bytesPerRow * 2) << 16) & 0x3FFFE000) |
		(mode.bytesPerRow & 0x00001fff));

	// CR69, bit 7 = 1 to use MM streams processor registers to control primary
	// stream.

	WriteCrtcReg(0x69, 0x80, 0x80);

	WriteReg32(S3_GLOBAL_GBD_REG, bci_enable | S3_LITTLE_ENDIAN | S3_BD64);

	// If MS1NB style linear tiling mode.
	// bit MM850C[15] = 0 select NB linear tile mode.
	// bit MM850C[15] = 1 select MS-1 128-bit non-linear tile mode.

	uint32 ulTmp = ReadReg32(ADVANCED_FUNC_CTRL) | 0x8000;	// use MS-s style tile mode
	WriteReg32(ADVANCED_FUNC_CTRL, ulTmp);

	// CR88, bit 4 - Block write enabled/disabled.
	//
	// Note: Block write must be disabled when writing to tiled
	//		 memory.  Even when writing to non-tiled memory, block
	//		 write should only be enabled for certain types of SGRAM.

	WriteCrtcReg(0x88, DISABLE_BLOCK_WRITE_2D, DISABLE_BLOCK_WRITE_2D);
}


static void 
Savage_SetGBD_MX(const DisplayModeEx& mode)
{
	SharedInfo& si = *gInfo.sharedInfo;
	int bci_enable = BCI_ENABLE;

	// CR67_3:
	//	= 1  stream processor MMIO address and stride register
	//		 are used to control the primary stream.
	//	= 0  standard VGA address and stride registers
	//		 are used to control the primary streams.

	WriteCrtcReg(0x67, 0x08, 0x08);
	// IGA 2.
	WriteSeqReg(0x26, 0x4f);		// select IGA 2 read/writes
	WriteCrtcReg(0x67, 0x08, 0x08);
	WriteSeqReg(0x26, 0x40);		// select IGA 1

	// Set primary stream to bank 0 via reg CRCA.
	WriteCrtcReg(MEMORY_CTRL0_REG, 0x00, MEM_PS1 + MEM_PS2);

	// MM81C0 and 81C4 are used to control primary stream.
	WriteReg32(PRI_STREAM_FBUF_ADDR0,  si.frameBufferOffset & 0x7fffff);
	WriteReg32(PRI_STREAM_FBUF_ADDR1,  si.frameBufferOffset & 0x7fffff);
	WriteReg32(PRI_STREAM2_FBUF_ADDR0, si.frameBufferOffset & 0x7fffff);
	WriteReg32(PRI_STREAM2_FBUF_ADDR1, si.frameBufferOffset & 0x7fffff);

	// Program Primary Stream Stride Register.
	//
	// Tell engine if tiling on or off, set primary stream stride, and
	// if tiling, set tiling bits/pixel and primary stream tile offset.
	// Note that tile offset (bits 16 - 29) must be scanline width in
	// bytes/128bytespertile * 256 Qwords/tile.  This is equivalent to
	// lDelta * 2.  Remember that if tiling, lDelta is screenwidth in
	// bytes padded up to an even number of tilewidths.

	WriteReg32(PRI_STREAM_STRIDE,
		(((mode.bytesPerRow * 2) << 16) & 0x3FFF0000) |
		(mode.bytesPerRow & 0x00003fff));
	WriteReg32(PRI_STREAM2_STRIDE,
		(((mode.bytesPerRow * 2) << 16) & 0x3FFF0000) |
		(mode.bytesPerRow & 0x00003fff));

	WriteReg32(S3_GLOBAL_GBD_REG, bci_enable | S3_LITTLE_ENDIAN | S3_BD64);

	// CR78, bit 3  - Block write enabled(1)/disabled(0).
	//		 bit 2  - Block write cycle time(0:2 cycles,1: 1 cycle)
	//	Note:	Block write must be disabled when writing to tiled
	//			memory.	Even when writing to non-tiled memory, block
	//			write should only be enabled for certain types of SGRAM.

	WriteCrtcReg(0x78, 0xfb, 0xfb);
}


static void 
Savage_SetGBD_Super(const DisplayModeEx& mode)
{
	SharedInfo& si = *gInfo.sharedInfo;
	int bci_enable = BCI_ENABLE_TWISTER;

	// CR67_3:
	//	= 1 stream processor MMIO address and stride register
	//		are used to control the primary stream.
	//	= 0 standard VGA address and stride registers
	//		are used to control the primary streams.

	WriteCrtcReg(0x67, 0x08, 0x08);
	// IGA 2.
	WriteSeqReg(0x26, 0x4f);		// select IGA 2 read/writes
	WriteCrtcReg(0x67, 0x08, 0x08);
	WriteSeqReg(0x26, 0x40);		// select IGA 1

	// Load ps1 active registers as determined by MM81C0/81C4.
	// Load ps2 active registers as determined by MM81B0/81B4.

	WriteCrtcReg(0x65, 0x03, 0x03);

	// Program Primary Stream Stride Register.
	//
	// Tell engine if tiling on or off, set primary stream stride, and
	// if tiling, set tiling bits/pixel and primary stream tile offset.
	// Note that tile offset (bits 16 - 29) must be scanline width in
	// bytes/128bytespertile * 256 Qwords/tile.  This is equivalent to
	// lDelta * 2.  Remember that if tiling, lDelta is screenwidth in
	// bytes padded up to an even number of tilewidths.

	WriteReg32(PRI_STREAM_STRIDE,
		(((mode.bytesPerRow * 2) << 16) & 0x3FFF0000) |
		(mode.bytesPerRow & 0x00001fff));
	WriteReg32(PRI_STREAM2_STRIDE,
		(((mode.bytesPerRow * 2) << 16) & 0x3FFF0000) |
		(mode.bytesPerRow & 0x00001fff));

	// MM81C0 and 81C4 are used to control primary stream.
	WriteReg32(PRI_STREAM_FBUF_ADDR0, si.frameBufferOffset);
	WriteReg32(PRI_STREAM_FBUF_ADDR1, 0x80000000);
	WriteReg32(PRI_STREAM2_FBUF_ADDR0, (si.frameBufferOffset & 0xfffffffc) | 0x80000000);
	WriteReg32(PRI_STREAM2_FBUF_ADDR1, si.frameBufferOffset & 0xfffffffc);

	// Bit 28:block write disable.
	WriteReg32(S3_GLOBAL_GBD_REG, bci_enable | S3_BD64 | 0x10000000);
}


static void 
Savage_SetGBD_2000(const DisplayModeEx& mode)
{
	SharedInfo& si = *gInfo.sharedInfo;
	int bci_enable = BCI_ENABLE_TWISTER;

	// MM81C0 and 81B0 are used to control primary stream.
	WriteReg32(PRI_STREAM_FBUF_ADDR0, si.frameBufferOffset);
	WriteReg32(PRI_STREAM2_FBUF_ADDR0, si.frameBufferOffset);

	// Program Primary Stream Stride Register.
	//
	// Tell engine if tiling on or off, set primary stream stride, and
	// if tiling, set tiling bits/pixel and primary stream tile offset.
	// Note that tile offset (bits 16 - 29) must be scanline width in
	// bytes/128bytespertile * 256 Qwords/tile.  This is equivalent to
	// lDelta * 2.  Remember that if tiling, lDelta is screenwidth in
	// bytes padded up to an even number of tilewidths.

	WriteReg32(PRI_STREAM_STRIDE,  ((mode.bytesPerRow << 4) & 0x7ff0));
	WriteReg32(PRI_STREAM2_STRIDE, ((mode.bytesPerRow << 4) & 0x7ff0));

	// CR67_3:
	//	= 1 stream processor MMIO address and stride register
	//		are used to control the primary stream.
	//	= 0 standard VGA address and stride registers
	//		are used to control the primary streams

	WriteCrtcReg(0x67, 0x08, 0x08);

	// Bit 28:block write disable.
	WriteReg32(S3_GLOBAL_GBD_REG, bci_enable | S3_BD64 | 0x10000000);

	WriteCrtcReg(0x73, 0x00, 0x20);		// CR73 bit 5 = 0 block write disable
}



static void 
Savage_SetGBD(const DisplayModeEx& mode)
{
	SharedInfo& si = *gInfo.sharedInfo;

	VerticalRetraceWait();

	switch (gInfo.sharedInfo->chipType) {
		case S3_SAVAGE_3D:
			Savage_SetGBD_3D(mode);
			break;

		case S3_SAVAGE_MX:
			Savage_SetGBD_MX(mode);
			break;

		case S3_SAVAGE4:
		case S3_PROSAVAGE:
		case S3_TWISTER:
		case S3_PROSAVAGE_DDR:
			Savage_SetGBD_Twister(mode);
			break;

		case S3_SUPERSAVAGE:
			Savage_SetGBD_Super(mode);
			break;

		case S3_SAVAGE2000:
			Savage_SetGBD_2000(mode);
			break;
	}

	WriteCrtcReg(0x50, 0xc1, 0xc1);		// CR50, bit 7,6,0 = 111, Use GBD

	// Set up the Global Bitmap Descriptor used for BCI.
	// Do not enable block_write because we can't determine if the memory
	// type is a certain type of SGRAM for which block write can be used.
	//	bit 24~25: tile format,  00 = linear
	//	bit 28: block write disble/enable, 0 = disable, 1 = enable

	si.globalBitmapDesc = mode.timing.h_display | (mode.bpp << 16)
		| TILE_FORMAT_LINEAR | BCI_BD_BW_DISABLE | S3_BD64;	// disable block write
}


static void 
Savage_Initialize2DEngine(const DisplayModeEx& mode)
{
	SharedInfo& si = *gInfo.sharedInfo;

	WriteCrtcReg(0x40, 0x01);	// enable graphics engine
	WriteCrtcReg(0x31, 0x0c);	// turn on 16-bit register access

	// Setup plane masks.
	WriteReg32(0x8128, ~0);		// enable all write planes
	WriteReg32(0x812C, ~0);		// enable all read planes
	WriteReg16(0x8134, 0x27);
	WriteReg16(0x8136, 0x07);

	switch (si.chipType) {
		case S3_SAVAGE_3D:
		case S3_SAVAGE_MX:

			WriteReg32(0x48C18, ReadReg32(0x48C18) & 0x3FF0);	// Disable BCI

			// Setup BCI command overflow buffer.  0x48c14
			// Bits 0-11  = Bits 22-11 of the Command Buffer Offset.
			// Bits 12-28 = Total number of entries in the command buffer(Read only).
			// Bits 29-31 = COB size index, 111 = 32K entries or 128K bytes
			WriteReg32(0x48C14, (si.cobOffset >> 11) | (si.cobSizeIndex << 29));

			// Program shadow status update.
			WriteReg32(0x48C10, 0x78207220);

			WriteReg32(0x48C0C, 0);
			// Enable BCI and command overflow buffer.
			WriteReg32(0x48C18, ReadReg32(0x48C18) | 0x0C);
			break;

		case S3_SAVAGE4:
		case S3_PROSAVAGE:
		case S3_TWISTER:
		case S3_PROSAVAGE_DDR:
		case S3_SUPERSAVAGE:

			// Some Savage4 and ProSavage chips have coherency problems with
			// respect to the Command Overflow Buffer (COB); thus, do not
			// enable the COB.

			WriteReg32(0x48C18, ReadReg32(0x48C18) & 0x3FF0);	// Disable BCI
			WriteReg32(0x48C10, 0x00700040);
			WriteReg32(0x48C0C, 0);
			WriteReg32(0x48C18, ReadReg32(0x48C18) | 0x08);		// enable BCI without COB

			break;

		case S3_SAVAGE2000:

			// Disable BCI.
			WriteReg32(0x48C18, 0);
			// Setup BCI command overflow buffer.
			WriteReg32(0x48C18, (si.cobOffset >> 7) | (si.cobSizeIndex));
			// Disable shadow status update.
			WriteReg32(0x48A30, 0);
			// Enable BCI and command overflow buffer.
			WriteReg32(0x48C18, ReadReg32(0x48C18) | 0x00280000);

			break;
	}

	// Use and set global bitmap descriptor.

	Savage_SetGBD(mode);
}


static void 
Savage_GEReset(const DisplayModeEx& mode)
{
	gInfo.WaitIdleEmpty();
	snooze(10000);

	for (int r = 1; r < 10; r++) {
		bool bSuccess = false;

		WriteCrtcReg(0x66, 0x02, 0x02);
		snooze(10000);
		WriteCrtcReg(0x66, 0x00, 0x02);
		snooze(10000);

		gInfo.WaitIdleEmpty();

		WriteReg32(DEST_SRC_STR, mode.bytesPerRow << 16 | mode.bytesPerRow);

		snooze(10000);
		switch (gInfo.sharedInfo->chipType) {
			case S3_SAVAGE_3D:
			case S3_SAVAGE_MX:
				bSuccess = (STATUS_WORD0 & 0x0008ffff) == 0x00080000;
				break;

			case S3_SAVAGE4:
			case S3_PROSAVAGE:
			case S3_PROSAVAGE_DDR:
			case S3_TWISTER:
			case S3_SUPERSAVAGE:
				bSuccess = (ALT_STATUS_WORD0 & 0x0081ffff) == 0x00800000;
				break;

			case S3_SAVAGE2000:
				bSuccess = (ALT_STATUS_WORD0 & 0x008fffff) == 0;
				break;
		}

		if (bSuccess)
			break;

		snooze(10000);
		TRACE("Savage_GEReset(), restarting S3 graphics engine reset %2d ...\n", r);
	}

	// At this point, the FIFO is empty and the engine is idle.

	WriteReg32(SRC_BASE, 0);
	WriteReg32(DEST_BASE, 0);
	WriteReg32(CLIP_L_R, ((0) << 16) | mode.timing.h_display);
	WriteReg32(CLIP_T_B, ((0) << 16) | mode.timing.v_display);
	WriteReg32(MONO_PAT_0, ~0);
	WriteReg32(MONO_PAT_1, ~0);

	Savage_SetGBD(mode);
}


static void 
Savage_CalcClock(long freq, int min_m,
				int min_n1, int max_n1,
				int min_n2, int max_n2,
				long freq_min, long freq_max,
				unsigned int *mdiv, unsigned int *ndiv, unsigned int *r)
{
	uint8 best_n1 = 16 + 2, best_n2 = 2, best_m = 125 + 2;

	double ffreq = freq / 1000.0 / BASE_FREQ;
	double ffreq_max = freq_max / 1000.0 / BASE_FREQ;
	double ffreq_min = freq_min / 1000.0 / BASE_FREQ;

	if (ffreq < ffreq_min / (1 << max_n2)) {
		TRACE("Savage_CalcClock() invalid frequency %1.3f Mhz\n", ffreq * BASE_FREQ);
		ffreq = ffreq_min / (1 << max_n2);
	}
	if (ffreq > ffreq_max / (1 << min_n2)) {
		TRACE("Savage_CalcClock() invalid frequency %1.3f Mhz\n", ffreq * BASE_FREQ);
		ffreq = ffreq_max / (1 << min_n2);
	}

	// Work out suitable timings.

	double best_diff = ffreq;

	for (uint8 n2 = min_n2; n2 <= max_n2; n2++) {
		for (uint8 n1 = min_n1 + 2; n1 <= max_n1 + 2; n1++) {
			int m = (int)(ffreq * n1 * (1 << n2) + 0.5);
			if (m < min_m + 2 || m > 127 + 2)
				continue;

			double div = (double)(m) / (double)(n1);
			if ((div >= ffreq_min) && (div <= ffreq_max)) {
				double diff = ffreq - div / (1 << n2);
				if (diff < 0.0)
					diff = -diff;
				if (diff < best_diff) {
					best_diff = diff;
					best_m = m;
					best_n1 = n1;
					best_n2 = n2;
				}
			}
		}
	}

	*ndiv = best_n1 - 2;
	*r = best_n2;
	*mdiv = best_m - 2;
}


static void 
Savage_WriteMode(const DisplayModeEx& mode, const SavageRegRec& regRec)
{
	TRACE("Savage_WriteMode() begin\n");

	SharedInfo& si = *gInfo.sharedInfo;

	gInfo.WaitIdleEmpty();

	WriteMiscOutReg(0x23);
	WriteCrtcReg(0x38, 0x48);		// unlock sys regs CR20~CR3F
	WriteCrtcReg(0x39, 0xa0);		// unlock sys regs CR40~CRFF
	WriteSeqReg(0x08, 0x06);		// unlock sequencer regs SR09~SRFF

	if ( ! S3_SAVAGE_MOBILE_SERIES(si.chipType))
		Savage_Initialize2DEngine(mode);

	if (ReadCrtcReg(0x66) & 0x01) {
		Savage_GEReset(mode);	// reset GE to make sure nothing is going on
	}

	WriteCrtcReg(0x67, regRec.CR67 & ~0x0e); // no STREAMS yet old and new

	// Set register SR19 to zero so that the ProSavage chips will start up
	// when booting under BeOS using the default boot screen, and set register
	// CR5F to zero so that the ProSavage chips will start up when Haiku boot
	// screen had a depth of 32 bits/pixel

	if (si.chipType == S3_PROSAVAGE || si.chipType == S3_TWISTER) {
		WriteSeqReg(0x19, 0);
		WriteCrtcReg(0x5f, 0);
	}

	// Clear bit 3 in SR30 so that Savage MX chip will startup.  If bit 3 is
	// not cleared, it will startup only if booting under BeOS using the
	// default boot screen or the boot screen resolution matches the resolution
	// of the mode currently being set.

	if (si.chipType == S3_SAVAGE_MX)
		WriteSeqReg(0x30, 0x00, 0x08);

	// Set extended regs.

	WriteCrtcReg(0x66, regRec.CR66);
	WriteCrtcReg(0x3a, regRec.CR3A);
	WriteCrtcReg(0x58, regRec.CR58);
	WriteCrtcReg(0x53, regRec.CR53 & 0x7f);

	// If Savage IX/MX or SuperSavage, set SR54 & SR56 to 0x10 so that when
	// resolutions are set where the width and/or height is less than the
	// native resolution of the attached LCD display, the chip will not expand
	// the display to fill the screen.  That is, if a resolution is set to
	// 640x480, it will use only 640x480 pixels for the display.  When the chip
	// expands the display, text is much less readable.

	if (S3_SAVAGE_MOBILE_SERIES(si.chipType)) {
		WriteSeqReg(0x54, 0x10);
		WriteSeqReg(0x56, 0x10);
	}

	// Set the standard CRTC vga regs.

	WriteCrtcReg(0x11, 0x00, 0x80);		// unlock CRTC reg's 0-7 by clearing bit 7 of cr11

	for (int j = 0; j < (int)B_COUNT_OF(regRec.CRTC); j++)
		WriteCrtcReg(j, regRec.CRTC[j]);

	// Setup HSYNC & VSYNC polarity.

	uint8 temp = ((ReadMiscOutReg() & 0x3f) | 0x0c);

	if (!(mode.timing.flags & B_POSITIVE_HSYNC))
		temp |= 0x40;
	if (!(mode.timing.flags & B_POSITIVE_VSYNC))
		temp |= 0x80;

	WriteMiscOutReg(temp);

	// Extended mode timing registers.
	WriteCrtcReg(0x53, regRec.CR53);
	WriteCrtcReg(0x5d, regRec.CR5D);
	WriteCrtcReg(0x5e, regRec.CR5E);
	WriteCrtcReg(0x3b, regRec.CR3B);
	WriteCrtcReg(0x3c, regRec.CR3C);
	WriteCrtcReg(0x43, regRec.CR43);
	WriteCrtcReg(0x65, regRec.CR65);

	// Restore the desired video mode with cr67.
	WriteCrtcReg(0x67, regRec.CR67 & ~0x0e);	// no streams for new and old streams engines

	// Other mode timing and extended regs.
	WriteCrtcReg(0x34, regRec.CR34);
	WriteCrtcReg(0x42, regRec.CR42);
	WriteCrtcReg(0x45, regRec.CR45);
	WriteCrtcReg(0x50, regRec.CR50);
	WriteCrtcReg(0x51, regRec.CR51);

	// Memory timings.
	VerticalRetraceWait();
	WriteCrtcReg(0x69, regRec.CR69);

	WriteCrtcReg(0x33, regRec.CR33);
	WriteCrtcReg(0x86, regRec.CR86);
	WriteCrtcReg(0x88, regRec.CR88);
	WriteCrtcReg(0x90, regRec.CR90);
	WriteCrtcReg(0x91, regRec.CR91);

	if (si.chipType == S3_SAVAGE4)
		WriteCrtcReg(0xb0, regRec.CRB0);

	WriteSeqReg(0x1b, regRec.SR1B);

	if ( ! (S3_SAVAGE_MOBILE_SERIES(si.chipType) &&  si.displayType == MT_LCD)) {
		// Set extended seq regs for dclk.
		WriteSeqReg(0x12, regRec.SR12);
		WriteSeqReg(0x13, regRec.SR13);
		WriteSeqReg(0x29, regRec.SR29);

		// Load new m, n pll values for dclk & mclk.
		temp = ReadSeqReg(0x15) & ~0x20;
		WriteSeqReg(0x15, temp);
		WriteSeqReg(0x15, temp | 0x20);
		WriteSeqReg(0x15, temp);
		snooze(100);
	}

	// Now write out cr67 in full, possibly starting STREAMS.
	VerticalRetraceWait();
	WriteCrtcReg(0x67, regRec.CR67);

	uint8 cr66 = ReadCrtcReg(0x66);
	WriteCrtcReg(0x66, cr66 | 0x80);
	uint8 cr3a = ReadCrtcReg(0x3a);
	WriteCrtcReg(0x3a, cr3a | 0x80);

	Savage_GEReset(mode);

	WriteCrtcReg(0x66, cr66);
	WriteCrtcReg(0x3a, cr3a);

	Savage_Initialize2DEngine(mode);

	WriteCrtcReg(0x40, 0x01);		// enable graphics engine

	Savage_SetGBD(mode);

	TRACE("Savage_WriteMode() done\n");

	return;
}


static bool 
Savage_ModeInit(const DisplayModeEx& mode)
{
	TRACE("Savage_ModeInit(%dx%d, %d kHz)\n",
		mode.timing.h_display, mode.timing.v_display, mode.timing.pixel_clock);

	SharedInfo& si = *gInfo.sharedInfo;
	SavageRegRec regRec;

	int horizScaleFactor = 1;

	if (mode.bpp == 16 && si.chipType == S3_SAVAGE_3D)
		horizScaleFactor = 2;

	InitCrtcTimingValues(mode, horizScaleFactor, regRec.CRTC,
			regRec.CR3B, regRec.CR3C, regRec.CR5D, regRec.CR5E);
	regRec.CRTC[0x17] = 0xEB;

	int dclk = mode.timing.pixel_clock;
	regRec.CR67 = 0x00;

	switch (mode.bpp) {
		case 8:
			if ((si.chipType == S3_SAVAGE2000) && (dclk >= 230000))
				regRec.CR67 = 0x10;		// 8bpp, 2 pixels/clock
			else
				regRec.CR67 = 0x00;		// 8bpp, 1 pixel/clock
			break;

		case 15:
			if (S3_SAVAGE_MOBILE_SERIES(si.chipType)
				|| ((si.chipType == S3_SAVAGE2000) && (dclk >= 230000)))
				regRec.CR67 = 0x30;		// 15bpp, 2 pixel/clock
			else
				regRec.CR67 = 0x20;		// 15bpp, 1 pixels/clock
			break;

		case 16:
			if (S3_SAVAGE_MOBILE_SERIES(si.chipType)
				|| ((si.chipType == S3_SAVAGE2000) && (dclk >= 230000)))
				regRec.CR67 = 0x50;		// 16bpp, 2 pixel/clock
			else
				regRec.CR67 = 0x40;		// 16bpp, 1 pixels/clock
			break;

		case 32:
			regRec.CR67 = 0xd0;
			break;
	}

	regRec.CR3A = (ReadCrtcReg(0x3a) & 0x7f) | 0x15;
	regRec.CR53 = 0x00;
	regRec.CR66 = 0x89;
	regRec.CR58 = (ReadCrtcReg(0x58) & 0x80) | 0x13;

	regRec.SR1B = ReadSeqReg(0x1b) | 0x10;	// enable 8-bit Color Lookup Table

	regRec.CR43 = regRec.CR45 = regRec.CR65 = 0x00;

	unsigned int m, n, r;
	Savage_CalcClock(dclk, 1, 1, 127, 0, 4, 180000, 360000, &m, &n, &r);
	regRec.SR12 = (r << 6) | (n & 0x3f);
	regRec.SR13 = m & 0xff;
	regRec.SR29 = (r & 4) | (m & 0x100) >> 5 | (n & 0x40) >> 2;

	TRACE("CalcClock, m: %d  n: %d  r: %d\n", m, n, r);

	regRec.CR42 = 0x00;
	regRec.CR34 = 0x10;

	int width = mode.bytesPerRow / 8;
	regRec.CR91 = 0xff & width;
	regRec.CR51 = (0x300 & width) >> 4;
	regRec.CR90 = 0x80 | (width >> 8);

	// Set frame buffer description.

	if (mode.bpp <= 8)
		regRec.CR50 = 0;
	else if (mode.bpp <= 16)
		regRec.CR50 = 0x10;
	else
		regRec.CR50 = 0x30;

	width = mode.timing.h_display;		// width of display in pixels

	if (width == 640)
		regRec.CR50 |= 0x40;
	else if (width == 800)
		regRec.CR50 |= 0x80;
	else if (width == 1024)
		regRec.CR50 |= 0x00;
	else if (width == 1152)
		regRec.CR50 |= 0x01;
	else if (width == 1280)
		regRec.CR50 |= 0xc0;
	else if (width == 1600)
		regRec.CR50 |= 0x81;
	else
		regRec.CR50 |= 0xc1;	// use GBD

	if (S3_SAVAGE_MOBILE_SERIES(si.chipType))
		regRec.CR33 = 0x00;
	else
		regRec.CR33 = 0x08;

	regRec.CR69 = 0;
	regRec.CR86 = ReadCrtcReg(0x86) | 0x08;
	regRec.CR88 = ReadCrtcReg(0x88) | DISABLE_BLOCK_WRITE_2D;
	regRec.CRB0 = ReadCrtcReg(0xb0) | 0x80;

	Savage_WriteMode(mode, regRec);		// write registers to set mode

	return true;
}



bool 
Savage_SetDisplayMode(const DisplayModeEx& mode)
{
	// The code to actually configure the display.
	// All the error checking must be done in ProposeDisplayMode(),
	// and assume that the mode values we get here are acceptable.

	WriteSeqReg(0x01, 0x20, 0x20);		// blank the screen

	if ( ! Savage_ModeInit(mode)) {
		TRACE("Savage_ModeInit() failed\n");
		return false;
	}

	Savage_AdjustFrame(mode);

	WriteSeqReg(0x01, 0x00, 0x20);		// unblank the screen

	return true;
}



void
Savage_AdjustFrame(const DisplayModeEx& mode)
{
	// Adjust start address in frame buffer.

	SharedInfo& si = *gInfo.sharedInfo;

	int address = (mode.v_display_start * mode.virtual_width)
			+ ((mode.h_display_start & ~0x3F) * (mode.bpp / 8));
	address &= ~0x1F;
	address += si.frameBufferOffset;

	switch (si.chipType) {
		case S3_SAVAGE_MX:
			WriteReg32(PRI_STREAM_FBUF_ADDR0, address & 0xFFFFFFFC);
			WriteReg32(PRI_STREAM_FBUF_ADDR1, address & 0xFFFFFFFC);
			break;
		case S3_SUPERSAVAGE:
			WriteReg32(PRI_STREAM_FBUF_ADDR0, 0x80000000);
			WriteReg32(PRI_STREAM_FBUF_ADDR1, address & 0xFFFFFFF8);
			break;
		case S3_SAVAGE2000:
			// Certain Y values seems to cause havoc, not sure why
			WriteReg32(PRI_STREAM_FBUF_ADDR0, (address & 0xFFFFFFF8));
			WriteReg32(PRI_STREAM2_FBUF_ADDR0, (address & 0xFFFFFFF8));
			break;
		default:
			WriteReg32(PRI_STREAM_FBUF_ADDR0, address | 0xFFFFFFFC);
			WriteReg32(PRI_STREAM_FBUF_ADDR1, address | 0x80000000);
			break;
	}

	return;
}


void 
Savage_SetIndexedColors(uint count, uint8 first, uint8* colorData, uint32 flags)
{
	// Set the indexed color palette for 8-bit color depth mode.

	(void)flags;		// avoid compiler warning for unused arg

	if (gInfo.sharedInfo->displayMode.space != B_CMAP8)
		return ;

	while (count--) {
		WriteIndexedColor(first++,	// color index
			colorData[0],			// red
			colorData[1],			// green
			colorData[2]);			// blue
		colorData += 3;
	}
}