#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/times.h>
#include <sys/time.h>
#include "libsys.h"
#include "gpio.h"
#include "mips_gfx.h"

#define PIC_NX			800
#define PIC_NY			600

gpio_if_t gpio_if;
box_t box;
gfx_t gfx;

uint32_t color_cycle[12] = {0x000000FF, 0x00808080, 0x0000FFFF, 0x00808080, 0x0000FF00, 0x00808080, 0x00FFFF00, 0x00808080, 0x00FF0000, 0x00808080, 0x00FF00FF, 0x00808080};

volatile uint32_t _g_blitter_rdy;
volatile uint32_t _g_blit_count;
void ISR_blitter_FIN(void)
{
	uint32_t volatile *pVGA_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_ADDR;
	uint32_t volatile *pVGA_dst = (uint32_t*)SYS_VGA_BLIT_DST_ADDR;
	uint32_t volatile *pVGA_dimx_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_DIMX;
	uint32_t volatile *pVGA_dimx_dst = (uint32_t*)SYS_VGA_BLIT_DST_DIMX;
	uint32_t volatile *pVGA_nx = (uint32_t*)SYS_VGA_BLIT_NX;
	uint32_t volatile *pVGA_ny = (uint32_t*)SYS_VGA_BLIT_NY;
	uint32_t volatile *pVGA_ctrl = (uint32_t*)SYS_VGA_CTRL;
	uint32_t volatile *pVGA_moffs_0 = (uint32_t*)SYS_VGA_FB_FRONT;
	uint32_t volatile *pVGA_const_color = (uint32_t*)SYS_VGA_BLIT_COLOR;
	uint32_t volatile *pGPIO = (uint32_t*)SYS_GPIO_0_DATA;
	static uint32_t color;

	// Ack
	if(_g_blit_count)
	{

		do
		{
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_FIN_ACK;
			*pVGA_const_color = color;
			color = ~color;
			_g_blit_count--;

		} while (!(*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY) & _g_blit_count);
	}
	else
	{
		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);

		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_FIN_ACK;
		_g_blitter_rdy = 1;
	}
//	fprintf(stderr, "ISR_blitter_FIN(): _g_blit_count=%d\n", _g_blit_count);
}

void ISR_blitter_EMPTY(void)
{
	uint32_t volatile *pVGA_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_ADDR;
	uint32_t volatile *pVGA_dst = (uint32_t*)SYS_VGA_BLIT_DST_ADDR;
	uint32_t volatile *pVGA_dimx_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_DIMX;
	uint32_t volatile *pVGA_dimx_dst = (uint32_t*)SYS_VGA_BLIT_DST_DIMX;
	uint32_t volatile *pVGA_nx = (uint32_t*)SYS_VGA_BLIT_NX;
	uint32_t volatile *pVGA_ny = (uint32_t*)SYS_VGA_BLIT_NY;
	uint32_t volatile *pVGA_ctrl = (uint32_t*)SYS_VGA_CTRL;
	uint32_t volatile *pVGA_moffs_0 = (uint32_t*)SYS_VGA_FB_FRONT;
	uint32_t volatile *pVGA_const_color = (uint32_t*)SYS_VGA_BLIT_COLOR;
	uint32_t volatile *pGPIO = (uint32_t*)SYS_GPIO_0_DATA;
	static uint32_t color;

	// Ack
	if(_g_blit_count)
	{

		do
		{
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_EMPTY_ACK;
			*pVGA_const_color = color;
			color = ~color;
			_g_blit_count--;

		} while (!(*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY) & _g_blit_count);
	}
	else
	{
		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);

		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_EMPTY_ACK;
		_g_blitter_rdy = 1;
	}
//	fprintf(stderr, "ISR_blitter_EMPTY(): _g_blit_count=%d\n", _g_blit_count);
}

void BlitWaitStillActive(void);
void BlitConstColor(uint32_t fb_addr, uint32_t dim_x, uint32_t pos_x, uint32_t pos_y, uint32_t size_x, uint32_t size_y, uint32_t color);
void Blit(uint32_t fb_addr, uint32_t dim_x, uint32_t pos_x_src, uint32_t pos_y_src, uint32_t pos_x_dst, uint32_t pos_y_dst, uint32_t size_x, uint32_t size_y);

// -------------------------------------------------------------
void LoadPic(uint64_t *pScreen, uint32_t screen_nx, uint32_t screen_ny, uint32_t *pImage, uint32_t image_nx, uint32_t image_ny, uint32_t image_scale_x, uint32_t image_scale_y)
{

	uint32_t off_x, off_y;
	uint32_t i, j, black;
	uint32_t *pScr, *pImg;

	off_x = (screen_nx-(image_scale_x*image_nx))/2;
	off_y = (screen_ny-(image_scale_y*image_ny))/2;

	black = 0;
	pScr = (uint32_t*)pScreen;
	for (i=0; i < screen_nx*screen_ny; i += 8)
	{
		__gfx_block_set_8(pScr, 0);
		pScr += 8;
	}

	if (!pImage)
		return;

	pScr = ((uint32_t*)pScreen) + screen_nx*off_y + off_x;
	pImg = pImage;

	for (i=0; i < image_ny*image_nx; i += 8)
	{
		__gfx_block_copy_8(pScr, pImg);
		pScr += 8;
		pImg += 8;
	}

/*
	uint32_t off_x, off_y;
	uint32_t i;
	uint32_t *pScr;

	off_x = (screen_nx-(image_scale_x*image_nx))/2;
	off_y = (screen_ny-(image_scale_y*image_ny))/2;

	memset(pScreen, 0, screen_nx*screen_ny*sizeof(uint32_t));
	pScr = ((uint32_t*)pScreen) + screen_nx*off_y + off_x;

	for (i=0; i < image_ny; i++)
	{
		memcpy(pScr, &pImage[i*image_nx], image_nx*sizeof(uint32_t));
		pScr += screen_nx;
	}
*/
}

int main (void)
{
	int result, i, j, cnt, size, num_runs;
	uint32_t volatile *pUART_baud = (uint32_t*)SYS_UART_BAUD;
	uint32_t volatile *pUART_stat = (uint32_t*)SYS_UART_STAT;
	uint32_t volatile *pGPIO = (uint32_t*)SYS_GPIO_0_DATA;
	uint32_t volatile *pReg_usec = (uint32_t*)SYS_TIMER_USEC;
	uint32_t volatile *pReg_sec = (uint32_t*)SYS_TIMER_SEC;
	uint32_t volatile *pSSRAM = (uint32_t*)SYS_SSRAM_IO;
	uint32_t volatile *pTim_ctrl = (uint32_t*)SYS_ITIM_CTRL;
	uint32_t volatile *pTim_stat = (uint32_t*)SYS_ITIM_STAT;
	uint32_t volatile *pTim0_cnt = (uint32_t*)SYS_ITIM0_CNT;
	uint32_t volatile *pTim0_cmp = (uint32_t*)SYS_ITIM0_CMP;
	uint32_t volatile *pTim1_cnt = (uint32_t*)SYS_ITIM1_CNT;
	uint32_t volatile *pTim1_cmp = (uint32_t*)SYS_ITIM1_CMP;
	uint32_t volatile *pVGA_cgcol = (uint32_t*)SYS_VGA_CGCOL;
	uint32_t volatile *pFlashPicture = (uint32_t*)(SYS_FLASH_MEM + 0x00600000);

	uint32_t volatile *pVGA_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_ADDR;
	uint32_t volatile *pVGA_dst = (uint32_t*)SYS_VGA_BLIT_DST_ADDR;
	uint32_t volatile *pVGA_dimx_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_DIMX;
	uint32_t volatile *pVGA_dimx_dst = (uint32_t*)SYS_VGA_BLIT_DST_DIMX;
	uint32_t volatile *pVGA_nx = (uint32_t*)SYS_VGA_BLIT_NX;
	uint32_t volatile *pVGA_ny = (uint32_t*)SYS_VGA_BLIT_NY;
	uint32_t volatile *pVGA_ctrl = (uint32_t*)SYS_VGA_CTRL;
	uint32_t volatile *pVGA_moffs_0 = (uint32_t*)SYS_VGA_FB_FRONT;
	uint32_t volatile *pVGA_const_color = (uint32_t*)SYS_VGA_BLIT_COLOR;

	uint32_t screen_res_x, screen_res_y, screen_fps;
	uint32_t pos, posx, posy, xoff, yoff, color;
	uint64_t *pBuf64_0, *pBuf64_1;
	time_t curr_date;
	struct tm *pDate, date;
	uint32_t start, end;
	char sel[80];
	struct timeval time_sec;

	Screen_clr();
	gpio_init(&gpio_if, SYS_GPIO_0_BASE);

	*pVGA_cgcol = 0x00FFFFFF;

	setbuf(stdout, NULL);

	gpio_write(&gpio_if, GPIO_0_MDIO_RST, GPIO_0_ALIGN_MDIO, GPIO_DATA_OFFSET, 1);
	sleep(100);
	gpio_write(&gpio_if, GPIO_0_MDIO_RST, GPIO_0_ALIGN_MDIO, GPIO_DATA_OFFSET, 0);

	printf("\n");

	interrupt_enable(5);

	GFX_init(&gfx);

	num_runs = 5000*512;
	size = 1;
	while(1)
	{
		fprintf(stdout, "----------------------------------------------------\n");
		fprintf(stdout, "Size = %d\n", size);
		fprintf(stdout, "----------------------------------------------------\n");

		// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_FIN_INTEN;
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_EMPTY_INTEN;

		box_create(&box, size, size);

		start = clock();
		for (i=0; i < num_runs/(8*size); i++)
		{
			GFX_box_draw_direct(&gfx, &box, 5, 5, color_cycle[i%12]);
		}

		end = clock();
		fprintf(stdout, "Software draw: Fillrate %f rect/s\n", (float)i/((float)(end-start)/CLOCKS_PER_SEC));
		sleep(500);

		// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		*pVGA_dimx_src0 = 800;
		*pVGA_dimx_dst = 800;
		*pVGA_nx = box.w;
		*pVGA_ny = box.h;
		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_OP_CONSTCOL;
		*pVGA_dst = (uint32_t)*pVGA_moffs_0 + 4*(5 + 800*5);// + 4*((uint32_t)rand()%800 + 800*((uint32_t)rand()%600));

		color = 0;
		start = clock();
		for (i=0; i < num_runs/size; i++)
		{
			// Blitter test
			while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
			*pVGA_const_color = color;
			color = ~color;
		}
		BlitWaitStillActive();
		end = clock();

		fprintf(stdout, "Hardware draw (polling): Fillrate %f rect/s\n", (float)i/((float)(end-start)/CLOCKS_PER_SEC));
		sleep(500);

		// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		interrupt_register(5, (void*)ISR_blitter_FIN);
		interrupt_enable(5);
		_g_blit_count = num_runs/size;
		_g_blitter_rdy = 0;


		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_FIN_ACK)
		{
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_FIN_ACK;
		}

		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_FIN_INTEN;
		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_FIN_ACK;

		start = clock();
		while (!_g_blitter_rdy);
		end = clock();

		fprintf(stdout, "Hardware draw (interrupt, single): Fillrate %f rect/s\n", (float)i/((float)(end-start)/CLOCKS_PER_SEC));
		sleep(500);
		interrupt_disable(5);
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_FIN_INTEN;
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_EMPTY_INTEN;

		// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		interrupt_register(5, (void*)ISR_blitter_EMPTY);
		interrupt_enable(5);
		_g_blit_count = num_runs/size;
		_g_blitter_rdy = 0;


		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_EMPTY_ACK)
		{
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_EMPTY_ACK;
		}

		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_EMPTY_INTEN;
		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_EMPTY_ACK;

		start = clock();
		while (!_g_blitter_rdy);
		end = clock();

		fprintf(stdout, "Hardware draw (interrupt, multi): Fillrate %f rect/s\n", (float)i/((float)(end-start)/CLOCKS_PER_SEC));
		sleep(500);

		interrupt_disable(5);
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_FIN_INTEN;
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_EMPTY_INTEN;

	//	GFX_set_background(&gfx, pFlashPicture, 800, 600, 1, 1);
		posx = posy = 0;
		GFX_show(&gfx);

		color = 0;
		for (i=0; i < 32; i++)
		{
			GFX_frame(&gfx, GFX_FRAME_SYNC);

			color = ~color;
			box_create(&box, 64-2*i, 64-2*i);
			xoff = 100;
			yoff = 0;
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff+i, color);
			BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+100+i, yoff+i, box.w, box.h, color);
			BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+201+i, yoff+i, box.w, box.h, color);
			Blit((uint32_t)*pVGA_moffs_0, 800, xoff+i, yoff+i, xoff+300+i, yoff+i, box.w, box.h);
			Blit((uint32_t)*pVGA_moffs_0, 800, xoff+i, yoff+i, xoff+401+i, yoff+i, box.w, box.h);

			box_create(&box, 63-2*i, 63-2*i);
			xoff = 100;
			yoff = 100;
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff+i, color);
			BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+100+i, yoff+i, box.w, box.h, color);
			BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+201+i, yoff+i, box.w, box.h, color);
			Blit((uint32_t)*pVGA_moffs_0, 800, xoff+i, yoff+i, xoff+300+i, yoff+i, box.w, box.h);
			Blit((uint32_t)*pVGA_moffs_0, 800, xoff+i, yoff+i, xoff+401+i, yoff+i, box.w, box.h);

			xoff = 100;
			yoff = 200;
			box_create(&box, 65-2*i, 65-2*i);
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff+i, color);
			BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+100+i, yoff+i, box.w, box.h, color);
			BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+201+i, yoff+i, box.w, box.h, color);
			Blit((uint32_t)*pVGA_moffs_0, 800, xoff+i, yoff+i, xoff+300+i, yoff+i, box.w, box.h);
			Blit((uint32_t)*pVGA_moffs_0, 800, xoff+i, yoff+i, xoff+401+i, yoff+i, box.w, box.h);

		}
		GFX_frame(&gfx, GFX_FRAME_SYNC);

		xoff = 0;
		yoff = 300;
		box_create(&box, 1, 64);
		for (i=0; i < 64; i++)
		{
			color = color_cycle[i%12];
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff, color);
		}
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff, yoff+65, 64, 64);
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff+1, yoff+65+65, 64, 64);

		xoff = 81;
		yoff = 300;
		box_create(&box, 1, 64);
		for (i=0; i < 64; i++)
		{
			color = color_cycle[i%12];
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff, color);
		}
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff, yoff+65, 64, 64);
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff+1, yoff+65+65, 64, 64);

		xoff = 160;
		yoff = 300;
		box_create(&box, 1, 63);
		for (i=0; i < 63; i++)
		{
			color = color_cycle[i%12];
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff, color);
		}
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff, yoff+64, 63, 63);
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff+1, yoff+64+64, 63, 63);

		xoff = 241;
		yoff = 300;
		box_create(&box, 1, 63);
		for (i=0; i < 63; i++)
		{
			color = color_cycle[i%12];
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff, color);
		}
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff, yoff+64, 63, 63);
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff+1, yoff+64+64, 63, 63);

		xoff = 320;
		yoff = 300;
		box_create(&box, 1, 65);
		for (i=0; i < 65; i++)
		{
			color = color_cycle[i%12];
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff, color);
		}
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff, yoff+66, 65, 65);
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff+1, yoff+66+66, 65, 65);

		xoff = 401;
		yoff = 300;
		box_create(&box, 1, 65);
		for (i=0; i < 65; i++)
		{
			color = color_cycle[i%12];
			GFX_box_draw_direct(&gfx, &box, xoff+i, yoff, color);
		}
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff, yoff+66, 65, 65);
		Blit((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, xoff+1, yoff+66+66, 65, 65);

		xoff = 10;
		yoff = 500;
		box_create(&box, 64, 1);
		BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, box.w, box.h, 0xFFFFFFFF);
		BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+1, yoff+2, box.w, box.h, 0xFFFFFFFF);

		xoff = 80;
		yoff = 500;
		box_create(&box, 1, 64);
		BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff, yoff, box.w, box.h, 0xFFFFFFFF);
		BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+10, yoff, box.w, box.h, 0xFFFFFFFF);
		BlitConstColor((uint32_t)*pVGA_moffs_0, 800, xoff+21, yoff, box.w, box.h, 0xFFFFFFFF);

		size++;
		if (size > 256)
			size = 1;

	}
}

void BlitConstColor(uint32_t fb_addr, uint32_t dim_x, uint32_t pos_x, uint32_t pos_y, uint32_t size_x, uint32_t size_y, uint32_t color)
{

	uint32_t volatile *pVGA_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_ADDR;
	uint32_t volatile *pVGA_dst = (uint32_t*)SYS_VGA_BLIT_DST_ADDR;
	uint32_t volatile *pVGA_dimx_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_DIMX;
	uint32_t volatile *pVGA_dimx_dst = (uint32_t*)SYS_VGA_BLIT_DST_DIMX;
	uint32_t volatile *pVGA_nx = (uint32_t*)SYS_VGA_BLIT_NX;
	uint32_t volatile *pVGA_ny = (uint32_t*)SYS_VGA_BLIT_NY;
	uint32_t volatile *pVGA_ctrl = (uint32_t*)SYS_VGA_CTRL;
	uint32_t volatile *pVGA_moffs_0 = (uint32_t*)SYS_VGA_FB_FRONT;
	uint32_t volatile *pVGA_const_color = (uint32_t*)SYS_VGA_BLIT_COLOR;

//	fprintf(stdout, "BlitConstColor to (%d,%d), size (%d,%d) ...", pos_x, pos_y, size_x, size_y);

	*pVGA_const_color = color;
	*pVGA_dst = fb_addr + 4*(pos_x + pos_y*dim_x);
	*pVGA_dimx_dst = dim_x;
	*pVGA_nx = size_x - 0;
	*pVGA_ny = size_y - 0;

	while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	*pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);

//	fprintf(stdout, "done\n");
}

void Blit(uint32_t fb_addr, uint32_t dim_x, uint32_t pos_x_src, uint32_t pos_y_src, uint32_t pos_x_dst, uint32_t pos_y_dst, uint32_t size_x, uint32_t size_y)
{

	uint32_t volatile *pVGA_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_ADDR;
	uint32_t volatile *pVGA_dst = (uint32_t*)SYS_VGA_BLIT_DST_ADDR;
	uint32_t volatile *pVGA_dimx_src0 = (uint32_t*)SYS_VGA_BLIT_SRC0_DIMX;
	uint32_t volatile *pVGA_dimx_dst = (uint32_t*)SYS_VGA_BLIT_DST_DIMX;
	uint32_t volatile *pVGA_nx = (uint32_t*)SYS_VGA_BLIT_NX;
	uint32_t volatile *pVGA_ny = (uint32_t*)SYS_VGA_BLIT_NY;
	uint32_t volatile *pVGA_ctrl = (uint32_t*)SYS_VGA_CTRL;
	uint32_t volatile *pVGA_moffs_0 = (uint32_t*)SYS_VGA_FB_FRONT;
	uint32_t volatile *pVGA_const_color = (uint32_t*)SYS_VGA_BLIT_COLOR;

//	fprintf(stdout, "Blit from (%d,%d) to (%d,%d), size (%d,%d) ...", pos_x_src, pos_y_src, pos_x_dst, pos_y_dst, size_x, size_y);

	*pVGA_src0 = fb_addr + 4*(pos_x_src + pos_y_src*dim_x);
	*pVGA_dimx_src0 = dim_x;
	*pVGA_dst = fb_addr + 4*(pos_x_dst + pos_y_dst*dim_x);
	*pVGA_dimx_dst = dim_x;
	*pVGA_nx = size_x - 0;
	*pVGA_ny = size_y - 0;

	while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
	*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

//	fprintf(stdout, "done\n");

}

void BlitWaitStillActive(void)
{
	uint32_t volatile *pVGA_ctrl = (uint32_t*)SYS_VGA_CTRL;

	while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
}