#include "video_fb.h"#include "sm501.h"#include <string.h>#include "global.h"#include "comdef.h"int sm501mode = 7;		// 10:1024*768,75Hz; 7:800*600, 72Hzint 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 SM501_DEBUG 0#define RGB(r, g, b) ((unsigned long)(((r) << 16) | ((g) << 8) | (b)))#define CONFIG_SM501_REG_BASE     0x23e00000u_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);		Uart_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)			Uart_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				Uart_Printf("Warning: read value %#lx != %#lx write value, trying again!\n", readback, nData);		}	#endif}static int sm501_detect(void){	int sm501_device_id;	int result = 0;	sm501_device_id = FIELD_GET(regRead32(DEVICE_ID), DEVICE_ID, DEVICE_ID);			if (sm501_device_id != 0x0501) {		Uart_Printf("Silicon Motion 501 not detected!ID=%x\n",sm501_device_id);		result = -1;		return result;	}	else		Uart_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)			{