diff --git a/lib/CPUs/MIPS/bsp/examples/testbench.c b/lib/CPUs/MIPS/bsp/examples/testbench.c
index ae16ef9..8dad570 100644
--- a/lib/CPUs/MIPS/bsp/examples/testbench.c
+++ b/lib/CPUs/MIPS/bsp/examples/testbench.c
@@ -23,6 +23,147 @@ int g_i;
 
 extern int paranoia(int argc, char **argv);
 
+void LoadPic(UINT64 *pScreen, UINT32 screen_nx, UINT32 screen_ny, UINT32 *pImage, UINT32 image_nx, UINT32 image_ny, UINT32 image_scale_x, UINT32 image_scale_y)
+{
+	UINT32 off_x, off_y;
+	UINT32 i;
+	UINT32 *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));
+	pScr = ((UINT32*)pScreen) + screen_nx*off_y + off_x;
+
+	for (i=0; i < image_ny; i++)
+	{
+		memcpy(pScr, &pImage[i*image_nx], image_nx*sizeof(UINT32));
+		pScr += screen_nx;
+	}
+}
+
+typedef struct _sbox_t
+{
+	UINT32 w, h;
+	UINT32 x, y;
+	UINT32 *pSave;
+} box_t;
+
+typedef struct _sgfx_t
+{
+	UINT32 w, h;
+	UINT32 *pFB;
+} gfx_t;
+
+void GFX_init(gfx_t *pObj)
+{
+	UINT32 volatile *pVGA_moffs = (UINT32*)SYS_VGA_MOFFS;
+	UINT32 volatile *pVGA_resx = (UINT32*)SYS_VGA_RESX;
+	UINT32 volatile *pVGA_resy = (UINT32*)SYS_VGA_RESY;
+
+	pObj->pFB = (UINT32*)*pVGA_moffs;
+	pObj->w = *pVGA_resx;
+	pObj->h = *pVGA_resy;
+}
+
+void GFX_box_create(box_t *pObj, UINT32 w, UINT32 h)
+{
+	pObj->pSave = NULL;
+	pObj->w = w;
+	pObj->h = h;
+	pObj->x = -1;
+	pObj->y = -1;
+}
+
+/*
+void GFX_restore(gfx_t *pObj, box_t *pBox)
+{
+	if (!pBox->pSave)
+	{
+		malloc(w*h*sizeof(UINT32));
+
+		if (!((xmax-xmin)%8))
+		{
+			i = 0;
+			for (y=ymin; y < ymax; y += pObj->w)
+			{
+
+				for (x=xmin; x < xmax; x += 8)
+				{
+					pBox->pSave[i+0] = pObj->pFB[x + y + 0];
+					pBox->pSave[i+1] = pObj->pFB[x + y + 1];
+					pBox->pSave[i+2] = pObj->pFB[x + y + 2];
+					pBox->pSave[i+3] = pObj->pFB[x + y + 3];
+					pBox->pSave[i+4] = pObj->pFB[x + y + 4];
+					pBox->pSave[i+5] = pObj->pFB[x + y + 5];
+					pBox->pSave[i+6] = pObj->pFB[x + y + 6];
+					pBox->pSave[i+7] = pObj->pFB[x + y + 7];
+				}
+				i += 8;
+			}
+		}
+		else
+		{
+			i = 0;
+			for (y=ymin; y < ymax; y += pObj->w)
+			{
+				for (x=xmin; x < xmax; x++)
+				{
+					pBox->pSave[i++] = pObj->pFB[x + y];
+				}
+			}
+		}
+	}
+}
+*/
+
+void GFX_box_draw(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy, UINT32 color)
+{
+	UINT32 xmin, xmax, ymin, ymax, x, y, i;
+
+	xmin = posx;
+	xmax = pBox->w + posx;
+	if (xmax > pObj->w)
+		xmax = pObj->w;
+
+
+	ymin = pObj->w*posy;
+	ymax = pBox->h + posy;
+	if (ymax > pObj->h)
+		ymax = pObj->h;
+
+	ymax *= pObj->w;
+
+	if (!((xmax-xmin)%8))
+	{
+		for (y=ymin; y < ymax; y += pObj->w)
+		{
+
+			for (x=xmin; x < xmax; x += 8)
+			{
+				pObj->pFB[x + y + 0] = color;
+				pObj->pFB[x + y + 1] = color;
+				pObj->pFB[x + y + 2] = color;
+				pObj->pFB[x + y + 3] = color;
+				pObj->pFB[x + y + 4] = color;
+				pObj->pFB[x + y + 5] = color;
+				pObj->pFB[x + y + 6] = color;
+				pObj->pFB[x + y + 7] = color;
+			}
+			i += 8;
+		}
+		return;
+	}
+
+	for (y=ymin; y < ymax; y += pObj->w)
+	{
+		for (x=xmin; x < xmax; x++)
+		{
+			pObj->pFB[x + y] = color;
+		}
+	}
+}
+
 int IsEndianBig(void)
 {
 	UINT8 *pBuf;
@@ -42,9 +183,9 @@ int IsEndianBig(void)
 
 void handler3(void)
 {
-	volatile UINT32 *pUART_stat = (UINT32*)sys_uart_stat;
-	volatile UINT32 *pUART_data = (UINT32*)sys_uart_data;
-	volatile UINT32 *pLED = (UINT32*)sys_led_port;
+	volatile UINT32 *pUART_stat = (UINT32*)SYS_UART_STAT;
+	volatile UINT32 *pUART_data = (UINT32*)SYS_UART_DATA;
+	volatile UINT32 *pLED = (UINT32*)SYS_LED_PORT;
 
 	while((0x10 & *pUART_stat))
 	{
@@ -56,12 +197,15 @@ void handler3(void)
 }
 
 int g_cnt;
+box_t box;
+gfx_t gfx;
+
 void handler7(void)
 {
-	UINT32 volatile *pGPIO0 = (UINT32*)sys_gpio0;
-	UINT32 volatile *pTim_stat = (UINT32*)sys_itim_stat;
-	UINT32 volatile *pTim_ctrl = (UINT32*)sys_itim_ctrl;
-	UINT32 volatile *pTim0_cmp = (UINT32*)sys_itim0_cmp;
+	UINT32 volatile *pGPIO0 = (UINT32*)SYS_GPIO0;
+	UINT32 volatile *pTim_stat = (UINT32*)SYS_ITIM_STAT;
+	UINT32 volatile *pTim_ctrl = (UINT32*)SYS_ITIM_CTRL;
+	UINT32 volatile *pTim0_cmp = (UINT32*)SYS_ITIM0_CMP;
 	time_t curr_date;
 	struct tm *pDate;
 
@@ -73,6 +217,7 @@ void handler7(void)
 	{
 		*pTim_stat = 1;
 		*pGPIO0 = g_cnt++;
+		GFX_box_draw(&gfx, &box, (UINT32)rand()%800, (UINT32)rand()%600, (UINT32)rand());
 	}
 
 	if (*pTim_stat & 4)
@@ -515,8 +660,8 @@ int Test16_MemTest(void)
 	UINT8 *ram8 = NULL;
 	UINT16 *ram16 = NULL;
 	UINT32 *ram32 = NULL;
-	UINT32 volatile *pFlashIO = (UINT32*)sys_flash_io;
-	UINT32 volatile *pFlashMem = (UINT32*)sys_flash_mem;
+	UINT32 volatile *pFlashIO = (UINT32*)SYS_FLASH_IO;
+	UINT32 volatile *pFlashMem = (UINT32*)SYS_FLASH_MEM;
 	UINT32 volatile *pROM = (UINT32*)0xBFC00000;
 
 	UINT32 *pSrc32, *pDst32;
@@ -782,47 +927,28 @@ int Test16_MemTest(void)
 #define	OFFSET_X		((SCREEN_X-(SCALE_X*PIC_NX))/2)
 #define	OFFSET_Y		((SCREEN_Y-(SCALE_Y*PIC_NY))/2)
 
-void LoadPic(UINT64 *pScreen, UINT32 screen_nx, UINT32 screen_ny, UINT32 *pImage, UINT32 image_nx, UINT32 image_ny, UINT32 image_scale_x, UINT32 image_scale_y)
-{
-	UINT32 off_x, off_y;
-	UINT32 i;
-	UINT32 *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));
-	pScr = ((UINT32*)pScreen) + screen_nx*off_y + off_x;
-
-	for (i=0; i < image_ny; i++)
-	{
-		memcpy(pScr, &pImage[i*image_nx], image_nx*sizeof(UINT32));
-		pScr += screen_nx;
-	}
-}
-
 int main (void)
 {
 	int result, i, j, cnt, size;
-	UINT32 volatile *pUART_baud = (UINT32*)sys_uart_baud;
-	UINT32 volatile *pUART_stat = (UINT32*)sys_uart_stat;
-	UINT32 volatile *pReg = (UINT32*)sys_led_port;
-	UINT32 volatile *pReg_usec = (UINT32*)sys_timer_usec;
-	UINT32 volatile *pReg_sec = (UINT32*)sys_timer_sec;
-	UINT32 volatile *pSSRAM = (UINT32*)sys_ssram_io;
-	UINT32 volatile *pTim_ctrl = (UINT32*)sys_itim_ctrl;
-	UINT32 volatile *pTim_stat = (UINT32*)sys_itim_stat;
-	UINT32 volatile *pTim0_cnt = (UINT32*)sys_itim0_cnt;
-	UINT32 volatile *pTim0_cmp = (UINT32*)sys_itim0_cmp;
-	UINT32 volatile *pTim1_cnt = (UINT32*)sys_itim1_cnt;
-	UINT32 volatile *pTim1_cmp = (UINT32*)sys_itim1_cmp;
-	UINT32 volatile *pVGA_ctrl = (UINT32*)sys_vga_ctrl;
-	UINT32 volatile *pVGA_moffs = (UINT32*)sys_vga_moffs;
-	UINT32 volatile *pVGA_resx = (UINT32*)sys_vga_resx;
-	UINT32 volatile *pVGA_resy = (UINT32*)sys_vga_resy;
-	UINT32 volatile *pVGA_fps = (UINT32*)sys_vga_fps;
-	UINT32 volatile *pVGA_cgcol = (UINT32*)sys_vga_cgcol;
-	UINT32 volatile *pFlashPicture = (UINT32*)(sys_flash_mem + 0x00600000);
+	UINT32 volatile *pUART_baud = (UINT32*)SYS_UART_BAUD;
+	UINT32 volatile *pUART_stat = (UINT32*)SYS_UART_STAT;
+	UINT32 volatile *pReg = (UINT32*)SYS_LED_PORT;
+	UINT32 volatile *pReg_usec = (UINT32*)SYS_TIMER_USEC;
+	UINT32 volatile *pReg_sec = (UINT32*)SYS_TIMER_SEC;
+	UINT32 volatile *pSSRAM = (UINT32*)SYS_SSRAM_IO;
+	UINT32 volatile *pTim_ctrl = (UINT32*)SYS_ITIM_CTRL;
+	UINT32 volatile *pTim_stat = (UINT32*)SYS_ITIM_STAT;
+	UINT32 volatile *pTim0_cnt = (UINT32*)SYS_ITIM0_CNT;
+	UINT32 volatile *pTim0_cmp = (UINT32*)SYS_ITIM0_CMP;
+	UINT32 volatile *pTim1_cnt = (UINT32*)SYS_ITIM1_CNT;
+	UINT32 volatile *pTim1_cmp = (UINT32*)SYS_ITIM1_CMP;
+	UINT32 volatile *pVGA_ctrl = (UINT32*)SYS_VGA_CTRL;
+	UINT32 volatile *pVGA_moffs = (UINT32*)SYS_VGA_MOFFS;
+	UINT32 volatile *pVGA_resx = (UINT32*)SYS_VGA_RESX;
+	UINT32 volatile *pVGA_resy = (UINT32*)SYS_VGA_RESY;
+	UINT32 volatile *pVGA_fps = (UINT32*)SYS_VGA_FPS;
+	UINT32 volatile *pVGA_cgcol = (UINT32*)SYS_VGA_CGCOL;
+	UINT32 volatile *pFlashPicture = (UINT32*)(SYS_FLASH_MEM + 0x00600000);
 	UINT32 screen_res_x, screen_res_y, screen_fps;
 	UINT64 *pBuf64;
 	time_t curr_date;
@@ -837,9 +963,10 @@ int main (void)
 	screen_res_x = *pVGA_resx;
 	screen_res_y = *pVGA_resy;
 
-	*pVGA_cgcol = 0x0000C000;
-	*pVGA_ctrl &= ~sys_vga_bit_msten;
-	clr_screen();
+	*pVGA_cgcol = 0x00FFFFFF;
+	*pVGA_ctrl &= ~SYS_VGA_BIT_MSTEN;
+	Screen_clr();
+
 
 	pBuf64 = (UINT64*)malloc(screen_res_x*screen_res_y*sizeof(UINT32));
 
@@ -847,7 +974,7 @@ int main (void)
 
 	*pTim0_cnt = 0;
 	*pTim1_cnt = 0;
-	*pTim0_cmp = (UINT32)(rand() & 0x3FF);
+	*pTim0_cmp = 0x2000 + (UINT32)(rand() & 0x7FFF);
 	*pTim1_cmp = 100000000;
 	*pTim_stat = (1 << 2) | (1 << 0);
 	*pTim_ctrl = (3 << 2);
@@ -869,7 +996,7 @@ int main (void)
 
 	LoadPic(pBuf64, screen_res_x, screen_res_y, pFlashPicture, PIC_NX, PIC_NY, 1, 1);
 
-	*pVGA_ctrl |= sys_vga_bit_msten;
+	*pVGA_ctrl |= SYS_VGA_BIT_MSTEN;
 
 	printf("VGA status = %08X\n", *pVGA_ctrl);
 	printf("VGA screen = %d x %d @ %d fps\n", screen_res_x, screen_res_y, screen_fps);
@@ -921,9 +1048,16 @@ int main (void)
 			} while(toupper(sel[0]) == 'N');
 		}
 	}
-
 	Test16_MemTest();
+	GFX_box_create(&box, 32, 32);
+	GFX_init(&gfx);
 
+	start = clock();
+	for (i=0; i < 10000; i++)
+		GFX_box_draw(&gfx, &box, (UINT32)rand()%800, (UINT32)rand()%600, (UINT32)rand());
+	end = clock();
+	printf("Fillrate %f rect/s\n", 10000.f/((float)(end-start)/CLOCKS_PER_SEC));
+	sleep(2000);
 	interrupt_enable(7);
 	interrupt_enable(3);
 	*pTim_ctrl |= 3;
@@ -998,20 +1132,20 @@ int main (void)
 		paranoia(1, NULL);
 		printf("passed\n\n");
 
-		printf("-------------------------------------------\n");
-		printf("-- Mem Test -------------------------------\n");
-		printf("-------------------------------------------\n");
-		result = Test16_MemTest();
-		if (IS_ERROR(result))
-			break;
-		printf("passed\n\n");
+//		printf("-------------------------------------------\n");
+//		printf("-- Mem Test -------------------------------\n");
+//		printf("-------------------------------------------\n");
+//		result = Test16_MemTest();
+//		if (IS_ERROR(result))
+//			break;
+//		printf("passed\n\n");
 
 		printf("Iteration %d: Passed\n", cnt);
 		curr_date = time(NULL);
 		pDate = gmtime(&curr_date);
 		puts(asctime(pDate));
 		cnt++;
-		*pTim0_cmp = (UINT32)(rand() & 0x3FF);
+		*pTim0_cmp = 0x2000 + (UINT32)(rand() & 0x7FFF);
 		*pTim_ctrl |= 1;
 
 //		printf("V=%.17e\n", pow((double)cnt-1, (double)cnt));