/*
 * vg_ip.cpp
 *
 */


//Video Generator try to workaround some FW bug:
//  1. DDR inaccessible: use FW internal block memory,configured at vp_ip_internal_memory_ with limited geometry of 512x512

#ifndef __MINGW32__
#define START_ENABLED
#endif

//#define FW_DDR_BUG_WORKAROUND	1
#define FW_RESET_BUG_WORKAROUND	1

#include "vg_driver.h"

#include "vg_ip_types.h"

#include "eif_discrete_signals.h"

#include "bsp_platform.h"
#include "bsp_interrupts.h"

#define NO_SC
#define NEW_IP_PALET_PATCH

#if FW_DDR_BUG_WORKAROUND==1
static void bsp_cache_data_flush(const void*, unsigned int)
{
}
#else
#include "bsp_cache.h"
#endif

#include <string.h>

//FW configuration
#define FW_BASE_ADDRESS		(0x50000000)

#define FW_CFG_ADDRESS_OLD		(FW_BASE_ADDRESS+0x00070000) //0x40010000
#define FW_PALET_ADDRESS_OLD	(FW_BASE_ADDRESS+0x00061000) //0x40001000
//#define FW_FB_ADDRESS		(FW_BASE_ADDRESS+0x00000000) //0x50000000 //0x40080000
#define FW_DAC_ADDRESS		(FW_BASE_ADDRESS+0x00080004) //0x40000004

#define FW_PALET_SC_ADDRESS_ANA	(FW_BASE_ADDRESS+0x00062000) //0x40001000

#define FW_CFG_ADDRESS_ANA		(FW_BASE_ADDRESS+0xC40000) //0x40010000
#define FW_PALET_ADDRESS_ANA	(FW_BASE_ADDRESS+0xC41000) //0x40001000

#define FW_PALET_SC_ADDRESS_DIG	(FW_BASE_ADDRESS+0x00062000)

#define FW_CFG_ADDRESS_DIG		(FW_BASE_ADDRESS+0xD40000)
#define FW_PALET_ADDRESS_DIG	(FW_BASE_ADDRESS+0xD41000)


enum { fw_palet_depth = 256 };

volatile vg_ip_fw_cfg_t vg_ip_fw_cfg_dummy__;	//public to be easy inspected with debugger
volatile vg_ip_fw_palet_t vg_ip_gfx_palet_dummy_;
volatile vg_ip_fw_palet_t vg_ip_sc_palet_dummy_;

volatile vg_ip_fw_cfg_t* vg_ip_cfg_=&vg_ip_fw_cfg_dummy__;
volatile vg_ip_fw_palet_t* vg_ip_graph_palet_=&vg_ip_gfx_palet_dummy_;
volatile vg_ip_fw_palet_t* vg_ip_sc_palet_=&vg_ip_sc_palet_dummy_;

struct vg_ip_internal_memory_t
{
	enum { dim = 640 };

	uint8_t data[dim][dim];
};

static bool vg_new_ip=true;

#ifdef __MINGW32__
static volatile unsigned int DUMMY_NO_CACHE;
vg_ip_internal_memory_t vg_ip_internal_memory_fake_;
#define VG_IP_INTERNAL_MEMORY (&vg_ip_internal_memory_fake_)

static bool vg_is_evm()
{
	return true; //bsp_is_evm();
}

static void dac_enable()
{

}

static unsigned int FW_IP_SELECTOR__;
#define FW_IP_SELECTOR (&FW_IP_SELECTOR__)

#else

#define FW_IP_SELECTOR	(0x500e07c8)

#define DUMMY_NO_CACHE	(*((volatile unsigned int*)(0x00100000)))
#define VG_IP_INTERNAL_MEMORY reinterpret_cast<vg_ip_internal_memory_t*>(0) //FW_FB_ADDRESS

static bool vg_is_evm()
{
	return bsp_is_evm();
}

static void dac_enable()
{
	if (vg_is_evm())
	{
		return;
	}
	GrifoEifDiscreteSignal::instance().setVideoDacEnable(true);
}

#endif

static bool vp_ip_digital_is_active()
{
	if (vg_is_evm())
		return false;
	unsigned int r=*reinterpret_cast<volatile unsigned int*>(FW_IP_SELECTOR);
	return r & 1 ? true : false;
}

static void vp_ip_digital_activate(bool digital)
{
	if (vg_is_evm())
		return;

	unsigned int r=*reinterpret_cast<volatile unsigned int*>(FW_IP_SELECTOR);
	if (digital)
		r=r|1;
	else
		r=r & (~1U);

	*reinterpret_cast<volatile unsigned int*>(FW_IP_SELECTOR)=r;
}

#define IP_BRAM_SIM	1

#ifdef IP_BRAM_SIM
vg_ip_internal_memory_t vg_ip_internal_memory_sim_store __attribute__((aligned(4096)));

//#define VG_IP_INTERNAL_MEMORY (&vg_ip_internal_memory_sim_);
vg_ip_internal_memory_t* vg_ip_internal_memory_sim_=&vg_ip_internal_memory_sim_store;
#else
vg_ip_internal_memory_t* vg_ip_internal_memory_sim_=0;
#endif

vg_ip_internal_memory_t* vg_ip_internal_memory_=VG_IP_INTERNAL_MEMORY;

unsigned int vg_ip_wdelay_=0;//no delay (new GC IP)
#define vg (*vg_ip_cfg_)

unsigned long vg_ip_fb_offset_;
volatile unsigned int vg_ip_dbg_no_update_;

volatile unsigned int vg_fw_delay__=1;

static void vg_delay_x(int n) __attribute__((noinline));

static void vg_delay_hard(int n)
{
	for(int i=0; i<n; ++i)
	{
		vg_fw_delay__+=DUMMY_NO_CACHE;
	}
}

static void vg_delay_x(int n)
{
	if (vg_ip_wdelay_==0)
		return;

	for(int i=0; i<n; ++i)
	{
		vg_fw_delay__+=DUMMY_NO_CACHE;
	}
}

static void vg_delay()
{
	vg_delay_x(10000);
}

static void vg_little_delay()
{
	vg_delay_x(1000);
}

static void vg_very_little_delay()
{
	vg_delay_x(100);
}

#if 0
struct vstd_param_t
{
	unsigned int id;
	unsigned int dx;
	unsigned int dy;
	const char* name;
	unsigned int interlaced;
	bool digital;
};
#endif
typedef vg_drv_vstd_param_t vstd_param_t;

#define VSTD_NUMS	10

static const vstd_param_t vstd_param[VSTD_NUMS]=
{
		{0, 764, 574, "STANAG3350-B", 1, false, "PAL 4:3"},	//4:3
		{1, 640, 484, "STANAG3350-C", 1, false, "NTSC 4:3"},	//4:3

		{2, 800, 600, "SVGA", 0, false, ""},
		{3, 1024, 768, "XGA", 0, false, ""},

		{4, 574, 574, "STANAG3350-B", 1, false, "PAL 1:1"},	//1:1
		{5, 484, 484, "STANAG3350-C", 1, false, "NTSC 1:1"},	//1:1

		{0, 1280, 720, "SMPTE-296M 60Hz", 0, true, ""},
		{1, 1280, 720, "SMPTE-296M 50Hz", 0, true, ""},
		{2, 1920, 1080, "SMPTE-274M 60Hz", 0, true, ""},
		{3, 1920, 1080, "SMPTE-274M 50Hz", 0, true, ""}
};

const vg_drv_vstd_param_t* vg_drv_vstd_info(int code)
{
	if ((code<0) || (code>=VSTD_NUMS))
		return 0;
    return &vstd_param[code];
}

struct vcfg_t
{
	const vstd_param_t* p;
	unsigned int dx;
	unsigned int dy;
	unsigned int wx;
	unsigned int wy;
	unsigned int ox;
	unsigned int oy;

	unsigned int stride;

	unsigned int size;

	unsigned int gfx_dx;
	unsigned int gfx_dy;
};

static vcfg_t vcfg;

static unsigned int r8(unsigned int x)
{
	x=x+7U;
	x=x & (~(7U));
	return x;
}

static uint32_t xy_encode(unsigned int x, unsigned int y)
{
	return x<<16|y;
}

static void geometry_setup(unsigned int vid, unsigned int dx, unsigned int dy, unsigned int stride)
{
	vcfg.p=&vstd_param[vid];
	vcfg.dx=vstd_param[vid].dx;
	vcfg.dy=vstd_param[vid].dy;

	vcfg.stride=r8(vcfg.dx);

	if (dx==0)
		dx=vcfg.dx;
	if (dy==0)
		dy=vcfg.dy;

	if (stride==0)
		stride=dx;

	if ((stride>vcfg.stride))
	{
		vcfg.stride=r8(stride);
		//dx=vcfg.dx;
	}

	if (dy>vcfg.dy)
		dy=vcfg.dy;

	vcfg.ox=(vcfg.dx-dx)/2;
	vcfg.oy=(vcfg.dy-dy)/2;
	vcfg.wx=dx;
	vcfg.wy=dy;

	vg.mode_selector=vg_ip_fw_cfg_t::mode_idle;

	vg.video_standard_selector=vstd_param[vid].id;
	//vg.graph_video_memory_offset=xy_encode(0, 0);
	unsigned int ox_fix=1; //GrifoEifDiscreteSignal::instance().hasNewVideoIp() ? 1 : 0;

	//vg.graph_window_offset=xy_encode(vcfg.ox+ox_fix, vcfg.oy);
	//vg.vmem_offset=xy_encode(vcfg.ox+ox_fix, vcfg.oy);

	vg.graph_window_size=xy_encode(vcfg.wx, vcfg.wy);

	vg.graph_memory_stride=vcfg.stride; //|(stride<<16);
	vg.graph_window_offset=xy_encode(ox_fix, 0);
	if (ox_fix)
		vg.vmem_offset=xy_encode(ox_fix, 0);

	vg.scan_window_size=xy_encode(512, 512);
	vg.scan_memory_stride=512;
	vg.scan_window_offset=0;

	vg.default_background_color=0x01010101;


	vg.configuration=ox_fix ? 0 : 0x800; //Enable a black back ground test pattern by default
		//The IP requires at least a plane that cover all the output resolution. In case of graphic window offset !=0,
	    //the test patter is used as back plane

	vcfg.size=dy*vcfg.stride;
}

void vg_drv_set_graphic_geometry(unsigned int dx, unsigned int dy)
{
	vcfg.gfx_dx=dx;
	vcfg.gfx_dy=dy;
}

void* vg_drv_configure(void* driver_memory_address, void* gfx_palet_address, void* sc_palet_address)
{
	if (driver_memory_address)
		vg_ip_cfg_=reinterpret_cast<vg_ip_fw_cfg_t*>(driver_memory_address);
	if (gfx_palet_address)
		vg_ip_graph_palet_=reinterpret_cast<vg_ip_fw_palet_t*>(gfx_palet_address);
	if (sc_palet_address)
	{
#ifndef NO_SC
		vg_ip_sc_palet_=reinterpret_cast<vg_ip_fw_palet_t*>(sc_palet_address);
#endif
	}
	return (void*)vg_ip_cfg_;
}

void* vg_drv_reconfigure(unsigned int flags, void* driver_memory_address, void* gfx_palet_address, void* sc_palet_address, void* fw_internal_fb)
{
	//if (bsp_is_evm())
	//	return (void*)vg_ip_cfg_;

	unsigned int no_updated=vg_ip_dbg_no_update_;

	vg_ip_dbg_no_update_=1;
#if 0
	if (driver_memory_address==0)
		driver_memory_address=reinterpret_cast<void*>(FW_CFG_ADDRESS_ANA);
	if (gfx_palet_address==0)
		gfx_palet_address=reinterpret_cast<void*>(FW_PALET_ADDRESS_ANA);
	if (sc_palet_address==0)
		sc_palet_address=reinterpret_cast<void*>(FW_PALET_SC_ADDRESS);
#endif

	bool new_ip=true; //WARNING: old IP no longer supported!!! GrifoEifDiscreteSignal::instance().hasNewVideoIp();

	if (new_ip)
	{
		vg_fw_delay__=0;
		if (driver_memory_address==0)
			driver_memory_address=reinterpret_cast<void*>(FW_CFG_ADDRESS_ANA);
		if (gfx_palet_address==0)
			gfx_palet_address=reinterpret_cast<void*>(FW_PALET_ADDRESS_ANA);
		//if (sc_palet_address==0)
		//	sc_palet_address=reinterpret_cast<void*>(FW_PALET_SC_ADDRESS);
	}
	else
	{
		if (driver_memory_address==0)
			driver_memory_address=reinterpret_cast<void*>(FW_CFG_ADDRESS_OLD);
		if (gfx_palet_address==0)
			gfx_palet_address=reinterpret_cast<void*>(FW_PALET_ADDRESS_OLD);
	}

	if (fw_internal_fb==0)
#ifdef FW_FB_ADDRESS
		fw_internal_fb=reinterpret_cast<void*>(FW_FB_ADDRESS);
#else
	fw_internal_fb=&vg_ip_internal_memory_sim_store; //reinterpret_cast<void*>(FW_FB_ADDRESS);
#endif


	vg_ip_internal_memory_=reinterpret_cast<vg_ip_internal_memory_t*>(fw_internal_fb);

	unsigned int ox_fix=0;

	if (new_ip)
	{
		vg_ip_internal_memory_=0;
		vg_ip_internal_memory_sim_=0;
		ox_fix=0x00010000;
		vg_new_ip=true;
	}

	if (vg_is_evm()) //bsp_is_evm())
	{
		vg_ip_dbg_no_update_=no_updated;
		return (void*)vg_ip_cfg_;
	}

	vg_drv_configure(driver_memory_address, gfx_palet_address, sc_palet_address);
	//vg_ip_fw_cfg_t* p=reinterpret_cast<vg_ip_fw_cfg_t*>(driver_memory_address);
	//vg_ip_cfg_=p;

	//*vg_ip_cfg_=vg_ip_fw_cfg_dummy__;
	if (flags & 1)
	{
		vg_ip_cfg_->mode_selector=0;
		vg_delay();

		unsigned int GDXY=vg_ip_fw_cfg_dummy__.graph_window_size;
		unsigned int GSXY=vg_ip_fw_cfg_dummy__.graph_memory_stride;
		unsigned int GOXY=vg_ip_fw_cfg_dummy__.graph_window_offset;
		unsigned int GMEM=vg_ip_fw_cfg_dummy__.graph_video_memory_offset;

		unsigned int SDXY=vg_ip_fw_cfg_dummy__.scan_window_size;
		unsigned int SSXY=vg_ip_fw_cfg_dummy__.scan_memory_stride;
		unsigned int SOXY=vg_ip_fw_cfg_dummy__.scan_window_offset;
		unsigned int SMEM=vg_ip_fw_cfg_dummy__.scan_video_memory_offset;

		if (vg_ip_internal_memory_)
		{
			const unsigned int DX=vg_ip_internal_memory_t::dim;
			const unsigned int DXY=DX|(DX<<16);
			//const unsigned int x_off=(800-512)/2;
			const unsigned int x_off=((vcfg.dx-DX)/2) & (~0x3);
			const unsigned int y_off=((vcfg.dy-DX)/2) & (~0x3);
			const unsigned int xy_off=y_off|(x_off<<16);

			GDXY=DXY;
			GSXY=DXY;
			GOXY=xy_off;
			GMEM=reinterpret_cast<unsigned int>(vg_ip_internal_memory_); //0;

			SDXY=DXY;
			SSXY=DXY;
			SOXY=xy_off;
			SMEM=0;
		}

		{//Copy configuration
			vg_ip_cfg_->graph_video_memory_offset=GMEM;
			vg_delay();

			vg_ip_cfg_->graph_memory_stride=GSXY;
			vg_delay();

			vg_ip_cfg_->graph_window_size=GDXY;
			vg_delay();

			vg_ip_cfg_->graph_window_offset=GOXY+ox_fix;
			vg_delay();

			if (new_ip)
			{
				vg_ip_cfg_->vmem_offset=GOXY+ox_fix;
				vg_delay();
			}

			vg_ip_cfg_->configuration=new_ip ? 0x09 : 0x809;
			vg_delay();
//#ifndef NO_SC
			vg_ip_cfg_->scan_video_memory_offset=SMEM;
			vg_delay();

			vg_ip_cfg_->scan_memory_stride=SSXY;
			vg_delay();

			vg_ip_cfg_->scan_window_offset=SOXY;
			vg_delay();

			vg_ip_cfg_->scan_window_size=SDXY;
			vg_delay();
//#endif
		}

		vg_delay();
		vg_ip_cfg_->video_standard_selector=vg_ip_fw_cfg_dummy__.video_standard_selector;
		vg_delay();
		vg_delay();

		vg_ip_cfg_->mode_selector=vg_ip_fw_cfg_dummy__.mode_selector;
		vg_delay();
	}

	if (flags & 4)
	{
		for(unsigned int i=0; i<fw_palet_depth; ++i)
		{
		  vg_ip_graph_palet_->graph_look_up_table.color[i]=vg_ip_gfx_palet_dummy_.graph_look_up_table.color[i];
		  vg_delay();
		  vg_ip_sc_palet_->graph_look_up_table.color[i]=vg_ip_sc_palet_dummy_.graph_look_up_table.color[i];
		  vg_little_delay();
		}
	}

	if (flags & 2)
		dac_enable(); //*reinterpret_cast<volatile uint32_t*>(FW_DAC_ADDRESS)=0x12;

	vg_ip_dbg_no_update_=no_updated;

	return (void*)vg_ip_cfg_;
}

#define IP_TEST_ALIGN	0


static vg_drv_standard_t vg_drv_std_default_=vg_std_stanag3350_b_11; //vg_std_svga;

void vg_drv_set_default_standard(vg_drv_standard_t std)
{
	vg_drv_std_default_=std;
}

int vg_drv_setup(enum vg_drv_standard_t phy_vid, void* fb, unsigned int dx, unsigned int dy, unsigned int stride)
{
	if (phy_vid==vg_std_default)
	{
		phy_vid=vg_drv_std_default_;
		int iphyvid=static_cast<int>(phy_vid);
		if ((iphyvid<0) || (iphyvid>(int)vg_std_stanag3350_c_11))
		{
			phy_vid=vg_std_svga;
		}
	}
	vg.mode_selector=vg_ip_fw_cfg_t::mode_idle;
	vg.configuration=vg_ip_fw_cfg_t::cfg_reset_value;

	geometry_setup(static_cast<unsigned int>(phy_vid), dx, dy, stride);

	vg.graph_video_memory_offset=reinterpret_cast<unsigned int>(fb);
#if 0
	struct vg_drv_palette_t* p=vg_drv_get_gfx_palette();
	p->color[0]=vg_drv_color_encode_gray(0);
	p->color[1]=vg_drv_color_encode_gray(0);

	for(unsigned int i=2; i<vg_drv_palette_t::num_colors; ++i)
	{
		p->color[i]=vg_drv_color_encode_gray(i);
	}
#else
	vg_drv_gfx_palette_set_gray_scale();
#endif

#if IP_TEST_ALIGN
	{
		uint32_t* raw=(uint32_t*)&vg;
		unsigned int w=(sizeof vg)/sizeof(uint32_t);
		for(unsigned int i=1; i<w; ++i)
		{
			raw[i]=i*4;
		}
	}
#endif

#ifdef START_ENABLED
	if (1) //!GrifoEifDiscreteSignal::instance().hasNewVideoIp())
	{
		vg_drv_reconfigure(0x0F, 0, 0, 0, 0);
		vg_drv_change_standard(phy_vid); //vg_std_stanag3350_b);p
		//vg_drv_video_enable(true);
	}
#endif

	return 0;
}

static int cfg_set(int enable, unsigned int cfg)
{
	int was_enable=vg.configuration & ~cfg;
	if (enable)
		vg.configuration&=~cfg;
	else
		vg.configuration|=cfg;
	return was_enable;
}

int vg_drv_video_enable(int enable)
{
	if (enable)
		vg.configuration|=vg_ip_fw_cfg_t::cfg_gfx_enable|vg_ip_fw_cfg_t::cfg_vid_enable;
	else
		vg.configuration&=~(vg_ip_fw_cfg_t::cfg_gfx_enable|vg_ip_fw_cfg_t::cfg_vid_enable);

	vg.mode_selector=vg_ip_fw_cfg_t::mode_active;
	return 0;
}

static int rt_test_pattern=0;
static unsigned int test_white_level;

int vg_drv_enable_test_pattern(int pattern, unsigned int white_level)
{
	rt_test_pattern=pattern;
	test_white_level=white_level;
	return pattern;
}

extern unsigned int vg_ip_internal_x_offset;
extern unsigned int vg_ip_interlaced;

unsigned int vg_ip_internal_x_offset=0;
unsigned int vg_ip_interlaced=0;

void vg_drv_set_framebuffer(const void* fb,unsigned int fb_size)
{
	if (vg_ip_dbg_no_update_)
		return;

	if (rt_test_pattern)
	{
		vg_drv_gen_test_pattern(rt_test_pattern, test_white_level, const_cast<void*>(fb), vcfg.dx, vcfg.dy, vcfg.stride); //800, 600, 0);
	}

	vg_ip_internal_memory_t* bram=vg_ip_internal_memory_sim_ ? vg_ip_internal_memory_sim_ : vg_ip_internal_memory_;

	if (bram)
	{
		unsigned int rows=vg_ip_internal_memory_t::dim;
		const uint8_t* src=reinterpret_cast<const uint8_t*>(fb);
		if (!rt_test_pattern)
			src+=vg_ip_internal_x_offset;

		if (vg_ip_interlaced)
		{
			rows=rows/2;
			for(unsigned int i=0; i<rows; ++i, src+=vcfg.stride*2)
			{
				memcpy(bram->data[i], src, sizeof bram->data[0]);
				memcpy(bram->data[i+rows], &src[vcfg.stride], sizeof bram->data[0]);
			}
		}
		else
		{
			for(unsigned int i=0; i<rows; ++i, src+=vcfg.stride)
			{
				memcpy(bram->data[i], src, sizeof bram->data[0]);
			}
		}

		if (vg_ip_internal_memory_ && (bram!=vg_ip_internal_memory_))
		{
			memcpy(vg_ip_internal_memory_->data, bram->data, sizeof vg_ip_internal_memory_->data);
		}
		else
			bsp_cache_data_flush(bram, sizeof *bram);
	}
	else
	{
		if (fb_size==0)
			fb_size=vcfg.size;
		bsp_cache_data_flush(fb, fb_size); //vcfg.size);

		unsigned long fb_raw=reinterpret_cast<unsigned long>(fb);
		fb_raw+=vg_ip_fb_offset_;
		//if (vg_ip_dbg_no_update_==0)
			vg.graph_video_memory_offset=fb_raw; //(unsigned long)fb;
	}

	//Dirty patch to war-around the DAC initialization problem: simple retry after some frames
	{
		static unsigned int dac_wake_up;
		if (dac_wake_up<200)
		{
			++dac_wake_up;
			if (dac_wake_up>=100)
				dac_enable(); //*reinterpret_cast<volatile uint32_t*>(FW_DAC_ADDRESS)=0x12;
		}
	}
}

static void vg_drv_set_palet_color(volatile vg_ip_fw_palet_t* p, int index, unsigned int color)
{
	if (index<fw_palet_depth)
	{
		if (vg_new_ip==false)
		{
			//Invert R/B:
			unsigned int r=color & 0x00FF;
			unsigned int b=(color>>16) & 0x00FF;
			color=(color & 0xFF00FF00);
			color=color | (b) | (r<<16);
		}
		if (index==0)
			color&=0x00FFFFFF;  //transparent
		else //if (index==1)
			color|=0xFF000000; //force opaque
		p->graph_look_up_table.color[index]=color;

		if (p!=vg_ip_sc_palet_)
			vg_ip_gfx_palet_dummy_.graph_look_up_table.color[index]=color;

		vg_very_little_delay();
	}
}


void vg_drv_set_gfx_color(unsigned int index, unsigned int color)
{
	vg_drv_set_palet_color(vg_ip_graph_palet_, index, color);
#if 0
	if (index<fw_palet_depth)
	{
		//Invert R/B:
		unsigned int r=color & 0x00FF;
		unsigned int b=(color>>16) & 0x00FF;
		color=(color & 0xFF00FF00);
		color=color | (b) | (r<<16);
		if (index==0)
			color&=0x00FFFFFF;  //transparent
		else //if (index==1)
			color|=0xFF000000; //force opaque
		vg_ip_graph_palet_->graph_look_up_table.color[index]=color;
	}
#endif
}

void vg_drv_set_sc_color(unsigned int index, unsigned int color)
{
	vg_drv_set_palet_color(vg_ip_sc_palet_, index, color);
}

struct vg_drv_palette_t* vg_drv_get_gfx_palette(void)
{
	return const_cast<vg_drv_palette_t*>(&vg_ip_graph_palet_->graph_look_up_table);
}

struct vg_drv_palette_t* vg_drv_get_sc_palette(void)
{
	return const_cast<vg_drv_palette_t*>(&vg_ip_sc_palet_->graph_look_up_table);
}


void vg_drv_gfx_palette_set_gray_scale(bool with_occlusion)
{
	volatile uint32_t* p=&(vg_ip_graph_palet_->graph_look_up_table.color[0]);
	volatile uint32_t* p1=&(vg_ip_sc_palet_->graph_look_up_table.color[0]);
	vg_ip_graph_palet_->graph_look_up_table.color[0]=0; //Transparent
	if (with_occlusion)
		vg_ip_graph_palet_->graph_look_up_table.color[1]=0xFF000000; //black opaque
	else
		vg_ip_graph_palet_->graph_look_up_table.color[1]=0xFF010101; //black opaque

	vg_ip_sc_palet_->graph_look_up_table.color[0]=0; //Transparent
	vg_ip_sc_palet_->graph_look_up_table.color[1]=0xFF000000; //black opaque

	for(int unsigned i=2; i<256; ++i)
	{
		uint32_t c=((uint32_t)i) & 0x0FF;
		uint32_t luminance=0xFF000000U;	//c<<24 - FW bug? it seems to be alpha channel, so we have to set it to 0xFF to be opaque
		uint32_t c32=luminance|c|(c<<8)|(c<<16);
		p[i]=c32;
		p1[i]=c32;

		//vg_ip_gfx_palet_dummy_.graph_look_up_table.color[i]=c32;

		vg_delay();
	}
}

void* vg_drv_get_fr_interal_fb()
{
	return vg_ip_internal_memory_;
}

vg_drv_raw_color_t vg_drv_color_encode(unsigned int red, unsigned int green, unsigned int blue)
{
	unsigned int luminance=(red >>2)+ (green>>1) + (blue>>2);
	return vg_drv_color_encode_luminance(luminance, red, green, blue);
}

vg_drv_raw_color_t vg_drv_color_encode_luminance(unsigned int luminance, unsigned int red, unsigned int green, unsigned int blue)
{
	//return luminance|(blue<<8)|(green<<16)|(red<<24);
	luminance=0xFF;
	uint32_t v=(blue<<16)|(green<8)|(red<<0)|(luminance<<24);
	return v;
}

vg_drv_raw_color_t vg_drv_color_encode_gray(unsigned int luminance)
{
	return vg_drv_color_encode_luminance(luminance, luminance, luminance, luminance);
}

int vg_drv_builtin_test_pattern_enable(int enable, enum vg_drv_builtin_test_pattern_t pattern_id)

{
	cfg_set(0, vg_ip_fw_cfg_t::cfg_test_pattern_enable);
	vg.configuration=vg.configuration & ~vg_ip_fw_cfg_t::cfg_test_pattern_mask;
	vg_ip_fw_reg_t tp=(pattern_id<<vg_ip_fw_cfg_t::cfg_test_pattern_shift) & vg_ip_fw_cfg_t::cfg_test_pattern_mask;
	vg.configuration|=tp;
	cfg_set(enable, vg_ip_fw_cfg_t::cfg_test_pattern_enable);

	return 0;
}


const vg_drv_vstd_param_t* vg_drv_change_standard(vg_drv_standard_t std_code, bool left_aligned)
{
	const vg_drv_vstd_param_t* std_name=0;

	unsigned int no_updated=vg_ip_dbg_no_update_;

	bool new_ip=true; //GrifoEifDiscreteSignal::instance().hasNewVideoIp();

	unsigned int std=static_cast<unsigned int>(std_code);
	if (std>9)
		std=5;

	vg_ip_dbg_no_update_=1;


	unsigned int ists=bsp_global_interrupts_disable();

	vg_ip_cfg_->mode_selector=vg_ip_fw_cfg_t::mode_idle;
	vg_delay();
	vg_ip_cfg_->configuration=0;
	vg_delay();

	const vstd_param_t& p=vstd_param[std];

	vcfg.p=&p;

	bool digital=p.digital;

	bool already_digital=vp_ip_digital_is_active();
	if (digital!=already_digital)
	{
		if (!bsp_is_evm())
		{
			unsigned int ists=bsp_interrupts_disable();

			unsigned int m=vg_ip_cfg_->graph_video_memory_offset;

			vp_ip_digital_activate(digital);

			vg_delay_hard(1000);

			if (digital)
			{
				vg_drv_reconfigure(0, (void*)FW_CFG_ADDRESS_DIG, (void*)FW_PALET_ADDRESS_DIG, 0, 0);
			}
			else
			{
				vg_drv_reconfigure(0, (void*)FW_CFG_ADDRESS_ANA, (void*)FW_PALET_ADDRESS_ANA, 0, 0);
			}

			vg_ip_cfg_->mode_selector=vg_ip_fw_cfg_t::mode_idle;
			vg_delay();
			vg_ip_cfg_->configuration=0;
			vg_delay();

			vg_ip_cfg_->default_background_color=0x01010101;

			vg_ip_cfg_->graph_video_memory_offset=m;

			for(int i=0; i<256; ++i)
			{
				vg_ip_graph_palet_->graph_look_up_table.color[i]=vg_ip_gfx_palet_dummy_.graph_look_up_table.color[i];
			}
			bsp_interrupts_restore(ists);
		}
	}

	unsigned int xmdim=vg_ip_internal_memory_->dim;
	unsigned int ymdim=vg_ip_internal_memory_->dim;

	unsigned int kmdim=vg_ip_internal_memory_->dim;

	if (vcfg.gfx_dx)
	{
		xmdim=vcfg.gfx_dx;
	}
	if (vcfg.gfx_dy)
	{
		ymdim=vcfg.gfx_dy;
	}

	bool x_too_small=p.dx<=xmdim;
	bool y_too_small=p.dy<=xmdim;

#if 0
	if (x_too_small)
	{
		unsigned int tmp=((mdim-p.dx))/2;
		tmp=(tmp+3) & (~(0x3));

		if (y_too_small)
		{
			tmp=tmp+800*tmp; //((mdim-p.dy)/2);
		}
		//tmp=(tmp+3) & (~(0x3));
		vg_ip_internal_x_offset=tmp;
	}
	else
		vg_ip_internal_x_offset=0;
#else
	vg_ip_internal_x_offset=0;
#endif

	unsigned int x_off=(x_too_small ? 0 : (p.dx-xmdim)/2) & (~(0x3));
	unsigned int y_off=(y_too_small ? 0 : (p.dy-ymdim)/2) & (~(0x3));

	unsigned int x_size=p.dx<=kmdim ? p.dx : kmdim;
	unsigned int y_size=p.dy<=kmdim ? p.dy : kmdim;

	if (y_size>512)
		y_size=512;

	if (left_aligned)
	{
		x_off=0;
		y_off=0;
	}

	x_off=x_off & (~1);
	y_off=y_off & (~1);

	if (p.interlaced && !new_ip)
	{
		y_size=y_size/2;
		y_off=y_off/2;
		vg_ip_interlaced=0;
		kmdim=kmdim*2;
	}
	else
		vg_ip_interlaced=0;

	if (new_ip)
		++x_off;

	vg_ip_cfg_->graph_window_offset=(x_off<<16)|y_off;
	vg_delay();
	if (new_ip)
	{
		vg_ip_cfg_->vmem_offset=(x_off<<16)|y_off;
		vg_delay();
		vg_ip_cfg_->graph_memory_stride=640; //vcfg.stride;
		vg_delay();
	}
	else
	{
		vg_ip_cfg_->graph_memory_stride=kmdim;
		vg_delay();
	}

	vg_ip_cfg_->graph_window_size=(x_size<<16)|(y_size);
	vg_delay();


	vg_ip_cfg_->video_standard_selector=p.id;
	vg_delay();

	vg_ip_cfg_->configuration=0x09;
	vg_delay();

	vg_ip_cfg_->mode_selector=vg_ip_fw_cfg_t::mode_active;
	vg_delay();

	vg_ip_dbg_no_update_=no_updated;

	vg_delay_hard(1000);

	bsp_global_interrupts_restore(ists);

	return std_name;
}


void vg_drv_resize(int pdx, int pdy, int stride)
{
	//vg_ip_cfg_->mode_selector=vg_ip_fw_cfg_t::mode_idle;

	vg_ip_cfg_->configuration=0;

	vg_very_little_delay();

	int y_center=0;
	int x_center=0;
	if (pdx<0)
	{
		x_center=1;
		pdx=-pdx;
	}
	if (pdy<0)
	{
		//y_center=1;
		pdy=-pdy;
	}
	unsigned int phy_dx=(unsigned int)pdx;
	unsigned int phy_dy=(unsigned int)pdy;

	if (vcfg.p)
	{
		if (phy_dy>vcfg.p->dy)
			phy_dy=vcfg.p->dy;
		if (phy_dx>vcfg.p->dx)
			phy_dx=vcfg.p->dx;
	}
	if (x_center)
	{
		x_center=(phy_dx-(unsigned int)pdx)/2;
	}

	unsigned int xy_center=((x_center+1)<<16)|y_center;

	vg_ip_cfg_->graph_memory_stride=stride;
	vg_ip_cfg_->vmem_offset=xy_center; //0x00010000;
	vg_ip_cfg_->graph_window_offset=xy_center; //0x00010000;

	vg_ip_cfg_->graph_window_size=phy_dy|(phy_dx<<16);

	//vg_ip_cfg_->mode_selector=vg_ip_fw_cfg_t::mode_active;

	vg_very_little_delay();

	vg_ip_cfg_->configuration=9;
}
