#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"

#define	TEST10_ERROR	(ERROR | 0x10)
#define	TEST11_ERROR	(ERROR | 0x11)
#define	TEST12_ERROR	(ERROR | 0x12)
#define	TEST13_ERROR	(ERROR | 0x13)
#define	TEST14_ERROR	(ERROR | 0x14)
#define	TEST15_ERROR	(ERROR | 0x15)
#define	TEST16_ERROR	(ERROR | 0x16)

#define PIC_NX			800
#define PIC_NY			600

char buffer[16384];
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;
	UINT32 word;
	word = 0x12345678;
	pBuf = (UINT8*)&word;
	if (((*pBuf+0) == 0x12) && (*(pBuf+1) == 0x34) && (*(pBuf+2) == 0x56) && (*(pBuf+3) == 0x78))
	{
		return 1;
	}
	if (((*pBuf+0) == 0x78) && (*(pBuf+1) == 0x56) && (*(pBuf+2) == 0x34) && (*(pBuf+3) == 0x12))
	{
		return 0;
	}
	return -1;
}

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;

	while((0x10 & *pUART_stat))
	{
		buffer[g_i] = (char)*pUART_data;
		_putchar(0, buffer[g_i]);
		g_i = (g_i+1)%sizeof(buffer);
	}

}

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;
	time_t curr_date;
	struct tm *pDate;

	if (*pTim0_cmp < 0x200)
	{
		*pTim0_cmp = 0x200;
	}
	if (*pTim_stat & 1)
	{
		*pTim_stat = 1;
		*pGPIO0 = g_cnt++;
		GFX_box_draw(&gfx, &box, (UINT32)rand()%800, (UINT32)rand()%600, (UINT32)rand());
	}

	if (*pTim_stat & 4)
	{
		*pTim_stat = 4;
		curr_date = time(NULL);
		pDate = gmtime(&curr_date);
		fprintf(stderr, "%s", asctime(pDate));
	}

}

int fibonacci(int f0, int f1, int *pDst, int len)
{
	int i;

	i = 0;

	pDst[i++] = f0;
	pDst[i++] = f1;

	for (; i < len; i++)
	{
		pDst[i] = pDst[i-2] + pDst[i-1];
	}

	return i;
}

void PrintCPUinfo(void)
{
	int result, rev_id;
	int cache_size, cache_line;
	int eb;

	char *cpu_type_str[7] = {"invalid", "R2000", "R3000", "R6000", "R4000", "reserved", "R6000A"};

	// Print Status register
	result = CP0_SR_read();

	printf("--------------------------------------------------\n");
	printf("Status   : %8.8X\n", result);

	// Print Revision
	result = CP0_PRID_read();
	rev_id = (result >> 8) & 0xFF;
	printf("CPU type : MIPS ");
	if ((rev_id > 0) && (rev_id < 0x07))
	{
		printf(cpu_type_str[rev_id]);
		printf(" Rev. ");
		rev_id = result & 0xFF;
		printf("%d", rev_id);
	}
	else
		printf("Unknown");

	eb = IsEndianBig();
	if (eb)
		printf(" [Big-endian]\n");
	if (!eb)
		printf(" [Little-endian]\n");
	if (eb < 0)
		printf(" [Unknown endian]\n");

	cache_size = (result >> 16) & 7;
	cache_line = (result >> 19) & 7;
	printf("I-Cache  : %d kBytes (%d Bytes per line)\n", 4*(int)pow(2.0, (double)(8+cache_size))/1024, 4*(int)pow(2.0, (double)(cache_line)));
	cache_size = (result >> 24) & 7;
	cache_line = (result >> 27) & 7;
	printf("D-Cache  : %d kBytes (%d Bytes per line)\n", 4*(int)pow(2.0, (double)(8+cache_size))/1024, 4*(int)pow(2.0, (double)(cache_line)));
	printf("--------------------------------------------------\n");

}

int non_aligned_load(UINT32* pMem)
{
	UINT32 result;
	__asm__
	(
		"li     $t0, 0x01234567\n"
		"li     $t1, 0x89ABCDEF\n"
		"sw     $t0, 0(%[pMem])\n"
		"sw     $t1, 64(%[pMem])\n"
		"li     %[val], 0x55555555\n"
		"lw     %[val], 0(%[pMem])\n"
		"lwl    %[val], 65(%[pMem])\n"
		: [val] "=r" (result)
		: [pMem] "r" (pMem)
	);
	return result;
}

int Test10_LoadStore()
{
	int *buf, i, result, data, err;
	volatile int *pPtr;

	buf = (int*)malloc(32*sizeof(int));
//	sputs("buf = ");
//	print_word((int)buf);
//	sputs("\n");

	while(1)
	{
		err = TEST10_ERROR;
		// Basic Load/Store
		pPtr = buf;
		*pPtr++ = 0x00000000;
		*pPtr++ = 0x55555555;
		*pPtr++ = 0xAAAAAAAA;
		*pPtr++ = 0xFFFFFFFF;
		pPtr = buf;
		if (*pPtr++ != 0x00000000)
			break;
		if (*pPtr++ != 0x55555555)
			break;
		if (*pPtr++ != 0xAAAAAAAA)
			break;
		if (*pPtr++ != 0xFFFFFFFF)
			break;

		// Filling from left
		pPtr = buf;
		data = 0x00000000;
		for (i=0; i < 32; i++)
		{
			*pPtr++ = data;
			data = data >> 1 | 0x80000000;

		}
		pPtr = buf;
		data = 0x00000000;
		for (i=0; i < 32; i++)
		{

			if (*pPtr++ != data)
				break;

			data = data >> 1 | 0x80000000;
		}
		if (i != 32)
			break;

		// Filling from right
		pPtr = buf;
		data = 0x00000000;
		for (i=0; i < 32; i++)
		{
			*pPtr++ = data;
			data = data << 1 | 1;

		}
		pPtr = buf;
		data = 0x00000000;
		for (i=0; i < 32; i++)
		{

			if (*pPtr++ != data)
				break;

			data = data << 1 | 1;
		}
		if (i != 32)
			break;

		// Walking ones
		pPtr = buf;
		data = 0x00000001;
		for (i=0; i < 32; i++)
		{
			*pPtr++ = data;
			data = data << 1;

		}
		pPtr = buf;
		data = 0x00000001;
		for (i=0; i < 32; i++)
		{

			if (*pPtr++ != data)
				break;

			data = data << 1;
		}
		if (i != 32)
			break;

		// Walking zeros
		pPtr = buf;
		data = 0xFFFFFFFE;
		for (i=0; i < 32; i++)
		{
			*pPtr++ = data;
			data = data << 1 | 1;

		}
		pPtr = buf;
		data = 0xFFFFFFFE;
		for (i=0; i < 32; i++)
		{

			if (*pPtr++ != data)
				break;

			data = data << 1 | 1;

		}
		if (i != 32)
			break;

		err = NO_ERROR;
		break;
	}
	err = TEST10_ERROR;
	data = non_aligned_load(buf);
	if (0 == IsEndianBig())
	{
		if (0xCDEF4567 == data)
			err = 0;
	}
	else
	{
		if (0xABCDEF67 == data)
			err = 0;
	}

	free(buf);

	return err;

}


#define	NUM_ELEMENTS	10000
#define	NUM_RUNS		3
int Test11_AddSub()
{
	int buf[NUM_ELEMENTS], result, i, j, diff, fill, num_right_elements, num_right_runs;

	fill = 0xAAAAAAAA;

	num_right_runs = 0;

	for (i=0; i < NUM_RUNS; i++)
	{
		for (j=0;  j < NUM_ELEMENTS; j++)
		{
			buf[j] = fill;
			fill = ((fill ^ j) - 1) << 1;
		}
		num_right_elements = 2;
		result = fibonacci(i, i+1, buf, NUM_ELEMENTS);
		for (j=2;  j < NUM_ELEMENTS; j++)
		{
			diff = buf[j] - buf[j-1];
			if (diff == buf[j-2])
				num_right_elements++;
		}
		if (num_right_elements == NUM_ELEMENTS)
			num_right_runs++;

	}

	if (num_right_runs != NUM_RUNS)
		return TEST11_ERROR;

	return NO_ERROR;
}

int Test12_MulDiv()
{
	int mix, i, j;
	int s1, s2, sp, st;
	div_t div_res;
	long cl;

	cl = (long)clock();
	srand(cl);
	for (i=0; i < 100; i++)
	{
		for (j=1; j < 100; j++)
		{
			mix = (int)rand();
			mix = (mix << 8) ^ (mix << 16);
			s1 = (int)rand() ^ mix;
			mix = (int)rand();
			mix = (mix << 8) ^ (mix << 16);
			s2 = (int)rand() ^ mix;
			if (!s2)
			{
				fprintf(stderr, "Test12_MulDiv(): Division by zero!\n");
				fprintf(stderr, "Clock() was 0x%8.8X\n", cl);
				continue;
			}

			div_res = div(s1, s2);

			sp = s2*div_res.quot;
			st = div_res.rem + sp;
			if (st != s1)
				return TEST12_ERROR;
		}
	}
	return NO_ERROR;
}

int Test13_COP0_Load_Store(void)
{
    int i,size;
	UINT32 *pSrc32, *pDst32;

	size = 0x2000;
	pSrc32 = (UINT32*)0xBFC00000;
	pDst32 = (UINT32*)calloc(size,sizeof(UINT32));
	for (i=0; i < size/4; i++)
	{
		CP0_TR_read_ptr(&pSrc32[i]);
		CP0_TR_write_ptr(&pDst32[i]);
	}
	for (i=size/4-1; i >= 0; i--)
		if (pDst32[i] != pSrc32[i])
			break;

	free(pDst32);
	i++;
	if (i)
		return TEST13_ERROR;

	return NO_ERROR;
}

int Test14_DCACHE_invalidate(void)
{
    int i,size;
	UINT32 *pSrc32, *pDst32;

	size = 0x2000;
	pSrc32 = (UINT32*)0xBFC00000;
	pDst32 = (UINT32*)calloc(size,sizeof(UINT32));
	for (i=0; i < size/4; i++)
	{
		pDst32[i] = pSrc32[i];
		DCACHE_invalidate_at(&pDst32[i]);
	}
	for (i=size/4-1; i >= 0; i--)
	{
		DCACHE_invalidate_at(&pDst32[i]);
		if (pDst32[i] != pSrc32[i])
			break;
	}

	free(pDst32);
	i++;
	if (i)
		return TEST14_ERROR;

	pDst32 = (UINT32*)calloc(size,sizeof(UINT32));
	for (i=0; i < size/4; i++)
	{
		pDst32[i] = pSrc32[i];
	}
	DCACHE_invalidate_all();
	for (i=size/4-1; i >= 0; i--)
		if (pDst32[i] != pSrc32[i])
			break;

	free(pDst32);
	i++;
	if (i)
		return TEST14_ERROR;

	return NO_ERROR;
}

#define PI_SCALE 10000
#define PI_MAXARR 2800
#define PI_ARRINIT 2000

int Test15_pi_calc()
{
	int i, j, k;
	int carry = 0;
	int arr[PI_MAXARR+1];
	UINT32 int_part, result;
	UINT16 pi_res[200] = {0};
	UINT16 pi_ref[200] =
	{
		3141,5926,5358,9793,2384,6264,3383,2795, 288,4197,1693,9937,5105,8209,7494,4592,3078,1640,6286,2089,9862,8034,8253,4211,
		7067,9821,4808,6513,2823, 664,7093,8446, 955, 582,2317,2535,9408,1284,8111,7450,2841, 270,1938,5211, 555,9644,6229,4895,
		4930,3819,6442,8810,9756,6593,3446,1284,7564,8233,7867,8316,5271,2019, 914,5648,5669,2346, 348,6104,5432,6648,2133,9360,
		7260,2491,4127,3724,5870, 660,6315,5881,7488,1520,9209,6282,9254, 917,1536,4367,8925,9036,  11,3305,3054,8820,4665,2138,
		4146,9519,4151,1609,4330,5727, 365,7595,9195,3092,1861,1738,1932,6117,9310,5118,5480,7446,2379,9627,4956,7351,8857,5272,
		4891,2279,3818,3011,9491,2983,3673,3624,4065,6643, 860,2139,4946,3952,2473,7190,7021,7986, 943,7027,7053,9217,1762,9317,
		6752,3846,7481,8467,6694, 513,2000,5681,2714,5263,5608,2778,5771,3427,5778,9609,1736,3717,8721,4684,4090,1224,9534,3014,
		6549,5853,7105, 792,2796,8925,8923,5420,1995,6112,1290,2196, 864, 344,1815,9813,6297,7477,1309,9605,1870,7211,3499,9999,
		8372,9780,4995,1059,7317,3281,6096,3185
	};

	//	setbuf(stdout, NULL);
	k = 0;
	for (i = 0; i <= PI_MAXARR; ++i)
		arr[i] = PI_ARRINIT;

	for (i = PI_MAXARR; i; i -= 14)
	{
		int sum = 0;
		for (j = i; j > 0; --j)
		{
			sum = sum*j + PI_SCALE*arr[j];
			arr[j] = sum % (j*2-1);
			sum /= (j*2-1);
		}
		result = carry + sum/PI_SCALE;
		int_part = PI_SCALE * (result / PI_SCALE);
		pi_res[k] = (UINT16)(result - int_part);
		carry = sum % PI_SCALE;
		k++;
	}

	for (k=0; k < sizeof(pi_ref)/sizeof(UINT16); k++)
	{
		printf("%d", pi_res[k]);
	}
	printf("\n");

	if(memcmp(pi_res, pi_ref, sizeof(pi_ref)))
		return TEST15_ERROR;

	return NO_ERROR;
}

#define	TEST_SIZE			(16*1024*1024)	// Bytes
#define	SMALL_TEST_SIZE		(8192)			// Bytes
#define	FLASH_SIZE			(8*1024*1024)	// Bytes

int Test16_MemTest(void)
{
	int result, i, j, cnt, size;
	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 *pROM = (UINT32*)0xBFC00000;

	UINT32 *pSrc32, *pDst32;
	UINT32 start, end;

	// ----------------------------------------------------------
	// Memtest BEGIN
/*	printf("SSRAM Memory Test\r\n");
	printf("Small data test\r\n");
	printf("Write (32-Bit access)...");
	start = clock();
	for (j=0; j < TEST_SIZE/SMALL_TEST_SIZE; j++)
		for (i=0; i < SMALL_TEST_SIZE/4; i++)
			pSSRAM[i] = (UINT32)i;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1000*(end-start)));

	PrintBuffer8((UINT8*)pSSRAM, 16, 256);

	printf("Verify (32-Bit access)...");
	start = clock();
	for (j=0; j < TEST_SIZE/SMALL_TEST_SIZE; j++)
		for (i=SMALL_TEST_SIZE/4-1; i >= 0; i--)
			if (pSSRAM[i] != (UINT32)i)
				break;

	end = clock();
	i++;
	if (i)
		printf("failed\r\n");
	else
		printf("passed (%.2f MByte/s)\n", (double)TEST_SIZE/(1000*(end-start)));
*/
	// ----------------------------------------------------------
	pSrc32 = (UINT32*)calloc(TEST_SIZE/4,sizeof(UINT32));
	pDst32 = (UINT32*)calloc(TEST_SIZE/4,sizeof(UINT32));

	// Memtest BEGIN
	printf("Dump first 16 bytes of flash blocks\n");
	for (i=0; i < FLASH_SIZE; i += 0x40000)
		memdump((UINT8*)(pFlashIO+i/4), 0, 16, 16);

	printf("\n");

	printf("SDRAM Memory Test\n");
	printf("Test size %d kByte\n\n", TEST_SIZE/1024);

	ram8 = (UINT8*)pDst32;
	printf("Zeroize SDRAM (memset)...");
	start = clock();
	memset(ram8, 0, TEST_SIZE);
	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	printf("Zeroize SDRAM (8-Bit access)...");
	start = clock();
	for (i=0; i < TEST_SIZE; i++)
		ram8[i] = (UINT8)0;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	ram16 = (UINT16*)pDst32;
	printf("Zeroize SDRAM (16-Bit access)...");
	start = clock();
	for (i=0; i < TEST_SIZE/2; i++)
		ram16[i] = (UINT16)0;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	ram32 = (UINT32*)pDst32;
	printf("Zeroize SDRAM (32-Bit access)...");
	start = clock();
	for (i=0; i < TEST_SIZE/4; i++)
		ram32[i] = (UINT32)0;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	ram8 = (UINT8*)pDst32;
	printf("Write SDRAM (8-Bit access)...");
	start = clock();
	for (i=0; i < TEST_SIZE; i++)
		ram8[i] = (UINT8)i;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	printf("Verify SDRAM (8-Bit access)...");
	start = clock();
	for (i=TEST_SIZE-1; i >= 0; i--)
		if (ram8[i] != (UINT8)i)
			break;

	end = clock();
	i++;
	if (i)
	{
		printf("failed\r\n");
		return TEST16_ERROR;
	}
	else
		printf("passed (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	ram16 = (UINT16*)pDst32;
	printf("Write SDRAM (16-Bit access)...");
	start = clock();
	for (i=0; i < TEST_SIZE/2; i++)
		ram16[i] = (UINT16)i;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	printf("Verify SDRAM (16-Bit access)...");
	start = clock();
	for (i=TEST_SIZE/2-1; i >= 0; i--)
		if (ram16[i] != (UINT16)i)
			break;

	end = clock();
	i++;
	if (i)
	{
		printf("failed\r\n");
		return TEST16_ERROR;
	}
	else
		printf("passed (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));


	ram32 = (UINT32*)pDst32;
	printf("Write SDRAM (32-Bit access)...");
	start = clock();
	for (i=0; i < TEST_SIZE/4; i++)
	{
		ram32[i] = (UINT32)i;
	}

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	printf("Verify SDRAM (32-Bit access)...");
	start = clock();
	for (i=TEST_SIZE/4-1; i >= 0; i--)
	{
		if (ram32[i] != (UINT32)i)
			break;
	}

	end = clock();
	i++;
	if (i)
	{
		printf("failed\r\n");
		return TEST16_ERROR;
	}
	else
		printf("passed (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	printf("\n");
	printf("Small data test (size = %d kByte)\n", SMALL_TEST_SIZE/1024);
	printf("Write SDRAM (32-Bit access)...");
	ram32 = (UINT32*)pDst32;
	start = clock();
	for (j=0; j < TEST_SIZE/SMALL_TEST_SIZE; j++)
		for (i=0; i < SMALL_TEST_SIZE/4; i++)
			ram32[i] = (UINT32)i;

	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	// -------------------------------------------------------------------
	// Verify
	printf("Verify SDRAM (32-Bit access)...");
	start = clock();
	for (j=0; j < TEST_SIZE/SMALL_TEST_SIZE; j++)
		for (i=SMALL_TEST_SIZE/4-1; i >= 0; i--)
			if (ram32[i] != (UINT32)i)
				break;

	end = clock();
	i++;
	if (i)
	{
		printf("failed\r\n");
		return TEST16_ERROR;
	}
	else
		printf("passed (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));

	for (i=1024; i < 4*65536; i *= 2)
	{
		printf("Memcpy() (%d kByte)...", i/1024);
		start = clock();
		for (j=0; j < TEST_SIZE/i; j++)
			memcpy(pDst32, pSrc32, i);

		end = clock();
		printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*(end-start)));
	}
	// -------------------------------------------------------------------
	printf("\n");

	printf("Memcpy() Flash(IO) => SDRAM \n");
	printf("Copying %d kBytes...", FLASH_SIZE/(1024));
	start = clock();
	memcpy(pDst32, pFlashIO, FLASH_SIZE);
	end = clock();
	printf("done (%.2f MByte/s)\n", (double)FLASH_SIZE/(1024*(end-start)));
	printf("\n");

	printf("Memcpy() Flash(MEM) => SDRAM \n");
	printf("Copying %d kBytes...", FLASH_SIZE/(1024));
	start = clock();
	memcpy(pDst32, pFlashMem, FLASH_SIZE);
	end = clock();
	printf("done (%.2f MByte/s)\n", (double)FLASH_SIZE/(1024*(end-start)));
	printf("\n");

	size = 0x2000;
	memcpy(pDst32, pROM, size);
	printf("Verify boot ROM...");
	for (i=size/4-1; i >= 0; i--)
		if (pDst32[i] != pROM[i])
			break;
	i++;
	if (i)
	{
		printf("failed\r\n");
		return TEST16_ERROR;
	}
	else
		printf("passed\r\n");

	printf("Verify FLASH(IO) <=> FLASH(MEM)...");
	if (!memcmp(pFlashIO, pFlashMem, FLASH_SIZE))
		printf("passed\r\n");
	else
	{
		printf("failed\r\n");
		return TEST16_ERROR;
	}

//	memdump((UINT8*)pSrc32, 0, 16, size);
//	memdump((UINT8*)0x40000000, 0, 16, 1024);

	// Memtest END
	// ----------------------------------------------------------
	free(pSrc32);
	free(pDst32);

	sputs("\r\n");
	return 0;
}

#define	SCREEN_X		screen_res_x
#define	SCREEN_Y		screen_res_y

#define	PIC_SCALE_X		1
#define	PIC_SCALE_Y		1
#define	OFFSET_X		((SCREEN_X-(SCALE_X*PIC_NX))/2)
#define	OFFSET_Y		((SCREEN_Y-(SCALE_Y*PIC_NY))/2)

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 screen_res_x, screen_res_y, screen_fps;
	UINT64 *pBuf64;
	time_t curr_date;
	struct tm *pDate, date;
	UINT32 start, end;
	char sel[80];
	struct timeval time_sec;


	g_cnt = 1;
	screen_fps = *pVGA_fps;
	screen_res_x = *pVGA_resx;
	screen_res_y = *pVGA_resy;

	*pVGA_cgcol = 0x00FFFFFF;
	*pVGA_ctrl &= ~SYS_VGA_BIT_MSTEN;
	Screen_clr();


	pBuf64 = (UINT64*)malloc(screen_res_x*screen_res_y*sizeof(UINT32));

	*pVGA_moffs = (UINT32)pBuf64;

	*pTim0_cnt = 0;
	*pTim1_cnt = 0;
	*pTim0_cmp = 0x2000 + (UINT32)(rand() & 0x7FFF);
	*pTim1_cmp = 100000000;
	*pTim_stat = (1 << 2) | (1 << 0);
	*pTim_ctrl = (3 << 2);

//	*pUART_baud = 1;

	setbuf(stdout, NULL);
	PrintCPUinfo();

	g_i = 0;
	memset(buffer, 0, sizeof(buffer));
	interrupt_register(3, handler3);
	fprintf(stderr, "UART interrupt registered.\n");

	interrupt_register(7, handler7);
	fprintf(stderr, "Timer interrupt registered.\n");

	printf("\n");

	LoadPic(pBuf64, screen_res_x, screen_res_y, pFlashPicture, PIC_NX, PIC_NY, 1, 1);

	*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);

	printf("Aktuelles Datum\n");
	curr_date = time(NULL);
	pDate = gmtime(&curr_date);
	puts(asctime(pDate));

	if (curr_date < 2)
	{
		printf("Datum und Uhrzeit setzen? [j/N]: ");
		scanf("%s", sel);
		if (toupper(sel[0]) == 'J')
		{
			do
			{
				printf("Datum\n");
			printf("Jahr  : ");
			scanf("%d", &date.tm_year);
			printf("Monat : ");
			scanf("%d", &date.tm_mon);
			printf("Tag   : ");
			scanf("%d", &date.tm_mday);

				printf("Uhrzeit\n");
				printf("Stunde  : ");
				scanf("%d", &date.tm_hour);
				printf("Minute  : ");
				scanf("%d", &date.tm_min);
				printf("Sekunde : ");
				scanf("%d", &date.tm_sec);

				date.tm_year  -= 1900;
				date.tm_mon  -= 1;
				time_sec.tv_sec = mktime(&date);
				time_sec.tv_usec = 0;
				settimeofday(&time_sec, NULL);

				printf("Datum\n");
				curr_date = time(NULL);
				pDate = gmtime(&curr_date);
				puts(asctime(pDate));
				printf("\nOK? [J/n]: ");
				scanf("%s", sel);
				printf("sel=%d\n", (int)sel[0]);


			} 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;
	*pUART_stat |= (1 << 6);
	srand(clock());

	cnt = 0;
	while (1)
	{
		for (i=0; i < 100000; i++)
		{
			interrupt_disable(7);
		}
		for (i=0; i < 100000; i++)
		{
			interrupt_enable(7);
		}

		*pReg = cnt & 0x3FFFFFFF;

		printf("-------------------------------------------\n");
		printf("-- LoadStore ------------------------------\n");
		printf("-------------------------------------------\n");
		result = Test10_LoadStore();
		if (IS_ERROR(result))
			break;
		printf("passed\n\n");

		printf("-------------------------------------------\n");
		printf("-- AddSub ---------------------------------\n");
		printf("-------------------------------------------\n");
		result = Test11_AddSub();
		if (IS_ERROR(result))
			break;
		printf("passed\n\n");

		printf("-------------------------------------------\n");
		printf("-- MulDiv ---------------------------------\n");
		printf("-------------------------------------------\n");
		result = Test12_MulDiv();
		if (IS_ERROR(result))
			break;
		printf("passed\n\n");

		printf("-------------------------------------------\n");
		printf("-- Cop0 LWZ0 and SWC0 ---------------------\n");
		printf("-------------------------------------------\n");
		result = Test13_COP0_Load_Store();
		if (IS_ERROR(result))
			break;
		printf("passed\n\n");

		printf("-------------------------------------------\n");
		printf("-- D-Cache invalidate ---------------------\n");
		printf("-------------------------------------------\n");
		result = Test14_DCACHE_invalidate();
		if (IS_ERROR(result))
			break;
		printf("passed\n\n");

		printf("-------------------------------------------\n");
		printf("-- PI calc --------------------------------\n");
		printf("-------------------------------------------\n");
		result = Test15_pi_calc();
		if (IS_ERROR(result))
			break;
		printf("passed\n\n");

		printf("-------------------------------------------\n");
		printf("-- Paranoia -------------------------------\n");
		printf("-------------------------------------------\n");
		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("Iteration %d: Passed\n", cnt);
		curr_date = time(NULL);
		pDate = gmtime(&curr_date);
		puts(asctime(pDate));
		cnt++;
		*pTim0_cmp = 0x2000 + (UINT32)(rand() & 0x7FFF);
		*pTim_ctrl |= 1;

//		printf("V=%.17e\n", pow((double)cnt-1, (double)cnt));
	}
	*pReg = 0x40000000 | cnt;
	fprintf(stderr, "Failed with error %8.8X\n", result);

	return 1;

}