sm5.c

基于S3C2410和SM501的彩屏控制器程序

#include "video_fb.h"
#include "sm501.h"
#include <string.h>
#include "global.h"

int sm501mode = 7;		// 10:1024*768,75Hz; 7:800*600, 72Hz
int sm501bpp = 16;
int	sm501out = 1;

GraphicDevice 			sm501;
struct SM501STRU		sm501info;
static char 			*sm501Reg;
static char 			*sm501Mem;
volatile unsigned int CONFIG_SM501_MEM_BASE = 0x20000000;
static int error;

#define FBTYPE_8BIT_INDEX        0
#define FBTYPE_8BIT_332RGB       5
#define FBTYPE_15BIT_555RGB      1
#define FBTYPE_16BIT_565RGB      2
#define FBTYPE_32BIT_X888RGB     3
#define FBTYPE_24BIT_888RGB      4
#define VSYNCTIMEOUT                       10000

#define RGB(r, g, b) ((unsigned long)(((r) << 16) | ((g) << 8) | (b)))
#define CONFIG_SM501_REG_BASE     0x23e00000

u_long regRead32(u_long nOffset);
void regWrite32(u_long nOffset, u_long nData);
// Format of mode table record.
typedef struct _mode_table_t
{
	// Horizontal timing.
	int			horizontal_total;
	int			horizontal_display_end;
	int			horizontal_sync_start;
	int			horizontal_sync_width;
	polarity_t	horizontal_sync_polarity;

	// Vertical timing.
	int 		vertical_total;
	int 		vertical_display_end;
	int 		vertical_sync_start;
	int 		vertical_sync_height;
	polarity_t	vertical_sync_polarity;

	// Refresh timing.
	long		pixel_clock;
	long		horizontal_frequency;
	long		vertical_frequency;
} mode_table_t, *pmode_table_t;

typedef struct _reg_table_t
{
	unsigned long	clock;
	unsigned long	control;
	unsigned long	fb_width;
	unsigned long	horizontal_total;
	unsigned long	horizontal_sync;
	unsigned long	vertical_total;
	unsigned long	vertical_sync;
	unsigned long	width;
	unsigned long	height;
	display_t		display;
} reg_table_t, *preg_table_t;

typedef struct clock_select_t
{
	long	mclk;
	int		divider;
	int		shift;
} clock_select_t, *pclock_select_t;

// Perform a rounded division.
long roundDiv(long num, long denom)
{
	/* n / d + 1 / 2 = (2n + d) / 2d */
	return (2 * num + denom) / (2 * denom);
}

// Finds clock closest to the requested.
long findClock(long requested_clock, clock_select_t *clock, display_t display)
{
	long	mclk;
	int		divider, shift;
	long	best_diff = 999999999;

	// Try 288MHz and 336MHz clocks.
	for (mclk = 288000000; mclk <= 336000000; mclk += 48000000)
	{
		// For CRT, try dividers 1 and 3, for panel, try divider 5 as well.
		for (divider = 1; divider <= (display == PANEL ? 5 : 3);
			 divider += 2)
		{
			// Try all 8 shift values.
			for (shift = 0; shift < 8; shift++)
			{
				// Calculate difference with requested clock.
				long diff = roundDiv(mclk, divider << shift) - requested_clock;
				if (diff < 0)
				{
					diff = -diff;
				}

				// If the difference is less than the current, use it.
				if (diff < best_diff)
				{
					// Store best difference.
					best_diff = diff;

					// Store clock values.
					clock->mclk = mclk;
					clock->divider = divider;
					clock->shift = shift;
				}
			}
		}
	}

	// Return best clock.
	return clock->mclk / (clock->divider << clock->shift);
}


/* Mode table. */
mode_table_t mode_table[] =
{
	/*----------------------------------------------------------------------------------------
	 * H.	H.    H.     H.   H.        V.   V.    V.    V.   V.        Pixel     H.     V.
	 * tot.	disp. sync   sync sync      tot. disp. sync  sync sinc      clock     freq.  freq.
	 *      end   start  wdth polarity       end   start hght polarity
	 *---------------------------------------------------------------------------------------*/

	/* 640 x 480 */
	{  800, 640,  656,   96,  NEGATIVE, 525, 480,  490,  2,   NEGATIVE, 25175000, 31469, 60 },
	{  832, 640,  664,   40,  NEGATIVE, 520, 480,  489,  3,   NEGATIVE, 31500000, 37861, 72 },
	{  840, 640,  656,   64,  NEGATIVE, 500, 480,  481,  3,   NEGATIVE, 31500000, 37500, 75 },
	{  832, 640,  696,   56,  NEGATIVE, 509, 480,  481,  3,   NEGATIVE, 36000000, 43269, 85 },

	/* 800 x 600 */
	{ 1024, 800,  824,   72,  POSITIVE, 625, 600,  601,  2,   POSITIVE, 36000000, 35156, 56 },
	{ 1056, 800,  840,  128,  POSITIVE, 628, 600,  601,  4,   POSITIVE, 40000000, 37879, 60 },
	{ 1040, 800,  856,  120,  POSITIVE, 666, 600,  637,  6,   POSITIVE, 50000000, 48077, 72 },
	{ 1056, 800,  816,   80,  POSITIVE, 625, 600,  601,  3,   POSITIVE, 49500000, 46875, 75 },
	{ 1048, 800,  832,   64,  POSITIVE, 631, 600,  601,  3,   POSITIVE, 56250000, 53674, 85 },

	/* 1024 x 768*/
	{ 1376, 1024, 1072, 96,  NEGATIVE, 808, 768,  769,  3,   NEGATIVE, 65000000, 47238, 60 },
	{ 1328, 1024, 1048, 136,  NEGATIVE, 806, 768,  771,  6,   NEGATIVE, 75000000, 56476, 70 },
	{ 1312, 1024, 1040,  96,  POSITIVE, 800, 768,  769,  3,   POSITIVE, 78750000, 60023, 75 },
	{ 1376, 1024, 1072,  96,  POSITIVE, 808, 768,  769,  3,   POSITIVE, 94500000, 68677, 85 },

	/* End of table. */
	{ 0, 0, 0, 0, NEGATIVE, 0, 0, 0, 0, NEGATIVE, 0, 0, 0 },
};

// Set DPMS state.
void setDPMS(DPMS_t state)
{
	unsigned long value;

	value = regRead32(SYSTEM_CTRL);
	switch (state)
	{
		case DPMS_ON:
			value = FIELD_SET(value, SYSTEM_CTRL, DPMS, VPHP);
			break;

		case DPMS_STANDBY:
			value = FIELD_SET(value, SYSTEM_CTRL, DPMS, VPHN);
			break;

		case DPMS_SUSPEND:
			value = FIELD_SET(value, SYSTEM_CTRL, DPMS, VNHP);
			break;

		case DPMS_OFF:
			value = FIELD_SET(value, SYSTEM_CTRL, DPMS, VNHN);
			break;
	}

	regWrite32(SYSTEM_CTRL, value);
}

void panelWaitVSync(int vsync_count)
{
	u_long status;
	u_long timeout;

	while (vsync_count-- > 0)
	{
		/* Wait for end of vsync */
		timeout = 0;
		do
		{
			status = FIELD_GET(regRead32(CMD_INTPR_STATUS),
							   CMD_INTPR_STATUS,
							   PANEL_SYNC);
			if (++timeout == VSYNCTIMEOUT)
				break;
		}
		while (status == CMD_INTPR_STATUS_PANEL_SYNC_ACTIVE);

		/* Wait for start of vsync */
		timeout = 0;
		do
		{
			status = FIELD_GET(regRead32(CMD_INTPR_STATUS),
							   CMD_INTPR_STATUS, 
							   PANEL_SYNC);
			if (++timeout == VSYNCTIMEOUT)
				break;
		}
		while (status == CMD_INTPR_STATUS_PANEL_SYNC_INACTIVE);
	}
}

void panelPowerSequence(int on_off, int vsync_delay)
{
	unsigned long panelControl = regRead32(PANEL_DISPLAY_CTRL);

	if (on_off == 1)
	{
		// Turn on FPVDDEN.
		panelControl = FIELD_SET(panelControl,
								 PANEL_DISPLAY_CTRL, FPVDDEN, HIGH);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
		panelWaitVSync(vsync_delay);

		// Turn on FPDATA.
		panelControl = FIELD_SET(panelControl, 
								 PANEL_DISPLAY_CTRL, DATA, ENABLE);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
		panelWaitVSync(vsync_delay);

		// Turn on FPVBIAS.
		panelControl = FIELD_SET(panelControl, 
								 PANEL_DISPLAY_CTRL, VBIASEN, HIGH);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
		panelWaitVSync(vsync_delay);

		// Turn on FPEN.
		panelControl = FIELD_SET(panelControl, 
								 PANEL_DISPLAY_CTRL, FPEN, HIGH);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
	}

	else
	{
		// Turn off FPEN.
		panelControl = FIELD_SET(panelControl,
								 PANEL_DISPLAY_CTRL, FPEN, LOW);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
		panelWaitVSync(vsync_delay);

		// Turn off FPVBIASEN.
		panelControl = FIELD_SET(panelControl, 
								 PANEL_DISPLAY_CTRL, VBIASEN, LOW);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
		panelWaitVSync(vsync_delay);

		// Turn off FPDATA.
		panelControl = FIELD_SET(panelControl, 
								 PANEL_DISPLAY_CTRL, DATA, DISABLE);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
		panelWaitVSync(vsync_delay);

		// Turn off FPVDDEN.
		panelControl = FIELD_SET(panelControl, 
								 PANEL_DISPLAY_CTRL, FPVDDEN, LOW);
		regWrite32(PANEL_DISPLAY_CTRL, panelControl);
	}
}


// Program new power mode.
void setPower(unsigned long nGates, unsigned long Clock)
{
	unsigned long gate_reg, clock_reg;
	unsigned long control_value;

	// Get current power mode.
	control_value = FIELD_GET(regRead32(POWER_MODE_CTRL),
							  POWER_MODE_CTRL,
							  MODE);

	switch (control_value)
	{
	case POWER_MODE_CTRL_MODE_MODE0:

		// Switch from mode 0 to mode 1.
		gate_reg = POWER_MODE1_GATE;
		clock_reg = POWER_MODE1_CLOCK;
		control_value = FIELD_SET(control_value,
					  POWER_MODE_CTRL, MODE, MODE1);
		break;

	case POWER_MODE_CTRL_MODE_MODE1:
	case POWER_MODE_CTRL_MODE_SLEEP:

		// Switch from mode 1 or sleep to mode 0.
		gate_reg = POWER_MODE0_GATE;
		clock_reg = POWER_MODE0_CLOCK;
		control_value = FIELD_SET(control_value, POWER_MODE_CTRL, MODE, MODE0);
		break;

	default:
		// Invalid mode
		return;
	}

	// Program new power mode.
	regWrite32(gate_reg, nGates);
	regWrite32(clock_reg, Clock);
	regWrite32(POWER_MODE_CTRL, control_value);

	// When returning from sleep, wait until finished.
	while (FIELD_GET(regRead32(POWER_MODE_CTRL),
					 POWER_MODE_CTRL,
					 SLEEP_STATUS) == POWER_MODE_CTRL_SLEEP_STATUS_ACTIVE) ;
}
u_long regRead32(u_long nOffset)
{
	#if SM501_DEBUG 
		u_long ret = *(volatile unsigned long *)(sm501Reg+nOffset);
		printf("%s: %#lx --> %#lx\n", __FUNCTION__, sm501Reg+nOffset, ret);
		return ret;
	#else
		return *(volatile unsigned long *)(sm501Reg+nOffset);
	#endif
}

void regWrite32(u_long nOffset, u_long nData)
{
	u_long readback;
	#if SM501_DEBUG 
	  	if (nOffset < 0x00080800)
			printf("%s: %#lx <-- %#lx\n", __FUNCTION__, nOffset, nData);

		*(volatile unsigned long *)(sm501Reg+nOffset) = nData;

		readback = *(volatile unsigned long *)(sm501Reg+nOffset);
		if (readback != nData)
		{
			printf("Warning: read value %#lx != %#lx write value, trying again!\n", readback, nData);
			error = 1;
		}
	#else
		for(;;)
		{
			*(volatile unsigned long *)(sm501Reg+nOffset) = nData;
			readback = *(volatile unsigned long *)(sm501Reg+nOffset);
			if (readback == nData)
				break;
			else
				printf("Warning: read value %#lx != %#lx write value, trying again!\n", readback, nData);
		}
	#endif
}

static int sm501_detect(void)
{
	int sm501_device_id = FIELD_GET(regRead32(DEVICE_ID), DEVICE_ID, DEVICE_ID);
	int result = 0;

	if (sm501_device_id != 0x0501) {
		printf("Silicon Motion 501 not detected\n");
		result = -1;
		return result;
	}
	else
		printf("Silicom Motion SM501 detected\n");
	return result;
}


// Program the mode with the registers specified.
void programMode(reg_table_t *register_table)
{
	unsigned long value, gate, clock;
	unsigned long palette_ram;
	unsigned long fb_size, offset;

	// Get current power configuration.
	gate = regRead32(CURRENT_POWER_GATE);
	clock = regRead32(CURRENT_POWER_CLOCK);

	// Program panel.
	if (register_table->display == PANEL)
	{
		// Program clock, enable display controller.
		gate = FIELD_SET(gate, CURRENT_POWER_GATE, DISPLAY, ENABLE);
		clock &= FIELD_CLEAR(CURRENT_POWER_CLOCK, P2XCLK_SELECT)
			  &  FIELD_CLEAR(CURRENT_POWER_CLOCK, P2XCLK_DIVIDER)
			  &  FIELD_CLEAR(CURRENT_POWER_CLOCK, P2XCLK_SHIFT);
		setPower(gate, clock | register_table->clock);

		// Calculate frame buffer address.
		value = 0;
		fb_size = register_table->fb_width * register_table->height;
		if (FIELD_GET(regRead32(CRT_DISPLAY_CTRL),
					  CRT_DISPLAY_CTRL,
					  PLANE) == CRT_DISPLAY_CTRL_PLANE_ENABLE)
		{
			value = FIELD_GET(regRead32(CRT_FB_ADDRESS),
							  CRT_FB_ADDRESS, 
							  ADDRESS);
			if (fb_size < value)
			{
				value = 0;
			}
			else
			{