#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <math.h>
#include <time.h>
#include <sys/times.h>
#include <sys/time.h>
#include <libsys.h>
#include <mips_gfx.h>
#include <console.h>
#include <assert.h>

#include <gpio.h>
#include <cop0.h>
#include <irq.h>
#include <mips_gfx.h>
#include <mips_ps2.h>

#define	TEST10_ERROR	(ERROR | 0x1000)
#define	TEST11_ERROR	(ERROR | 0x1100)
#define	TEST12_ERROR	(ERROR | 0x1200)
#define	TEST13_ERROR	(ERROR | 0x1300)
#define	TEST14_ERROR	(ERROR | 0x1400)
#define	TEST15_ERROR	(ERROR | 0x1500)
#define	TEST16_ERROR	(ERROR | 0x1600)

#define PIC_NX			800
#define PIC_NY			600

extern int paranoia(int argc, char **argv);
extern int calibrate_delay(void);

gpio_if_t gpio_if;
int g_cnt;
box_t box;
gfx_t gfx;
int posx, posy;

uint32_t* pf(void)
{
	static uint32_t p;

	return &p;
}

// -------------------------------------------------------------
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 IsEndianBig(void)
{
	uint8_t *pBuf;
	uint32_t word;
	word = 0x12345678;
	pBuf = (uint8_t*)&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(EXCEPTION_CONTEXT *xcp)
{
	volatile uint32_t *pUART_stat = (uint32_t*)SYS_UART_STAT;
	volatile uint32_t *pUART_data = (uint32_t*)SYS_UART_DATA;

	if((0x10 & *pUART_stat))
	{
		_putchar(1, (char)*pUART_data);
	}

}

void handler7(EXCEPTION_CONTEXT *xcp)
{
	uint32_t volatile *pTim_stat = (uint32_t*)SYS_ITIM_STAT;
	uint32_t volatile *pTim_ctrl = (uint32_t*)SYS_ITIM_CTRL;
	uint32_t volatile *pTim0_cmp = (uint32_t*)SYS_ITIM0_CMP;
	time_t curr_date;
	struct tm *pDate;

	if (*pTim_stat & 1)
	{
		*pTim_stat = 1;	// Acknowledge IRQ
		gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, g_cnt++);
		GFX_frame(&gfx, GFX_FRAME_NOSYNC);
		GFX_restore(&gfx, &box, posx, posy);
		GFX_show(&gfx);

		posx = (uint32_t)rand()%800;
		posy = (uint32_t)rand()%600;
		GFX_frame(&gfx, GFX_FRAME_NOSYNC);
		GFX_box_draw(&gfx, &box, posx, posy, (uint32_t)rand());
		GFX_show(&gfx);
		*pTim0_cmp = (uint32_t)(rand() & 0x000FFFFF);
		*pTim_ctrl |= 1;
	}

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

}

uint32_t uncached_cachemiss_bug(void)
{
	uint32_t result;
	uint32_t pMem[2];

	pMem[0] = 0x12345678;
	pMem[1] = 0xFFFFFFFF;

	__asm__
	(
		"cop0	34\n"
		"lui    $s0, 0xA000\n"
		"lw     $a0, 48($s0)\n"
		"or		$s1, $a0, $0\n"
		"or		$a2, $0, $0\n"
		"addiu	$a2, 0x7F00\n"
		"sw     $a2, 48($s0)\n"
//		"lw     $a1, 0(%[pMem])\n"
		"lw     $a0, 48($s0)\n"
		"lw     $a2, 4(%[pMem])\n"
		"lw     $a1, 0(%[pMem])\n"
		"nop\n"
		"sw     $s1, 48($s0)\n"
		"or		%[val], $a0, $0\n"
		: [val] "=r" (result)
		: [pMem] "r" (pMem)
		: "a0", "a1", "a2", "s0", "s1"
	);
	return result;
}

uint32_t non_aligned_load()
{
	uint32_t result;
	uint32_t pMem[3];

	__asm__
	(
		"li     $t0, 0x01234567\n"
		"li     $t1, 0x89ABCDEF\n"
		"sw     $t0, 0(%[pMem])\n"
		"sw     $t1, 4(%[pMem])\n"
		"li     %[val], 0x55555555\n"
		"lw     %[val], 0(%[pMem])\n"
		"lwl    %[val], 5(%[pMem])\n"
		: [val] "=r" (result)
		: [pMem] "r" (pMem)
		: "t0", "t1"
	);

	if (0 == IsEndianBig())
	{
		if (0xCDEF4567 != result)
			return ERROR;
	}
	else
	{
		if (0xABCDEF67 != result)
			return ERROR;
	}
	return 0;

}

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;
	}
	free(buf);

	if (IS_ERROR(err))
		return err;

	if (IS_ERROR(non_aligned_load()))
		return TEST10_ERROR | 1;

	if (IS_ERROR(uncached_cachemiss_bug()))
		return TEST10_ERROR | 2;

	return NO_ERROR;

}


#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_t *pSrc32, *pDst32;

	size = 0x2000;
	pSrc32 = (uint32_t*)0xBFC00000;
	pDst32 = (uint32_t*)calloc(size,sizeof(uint32_t));
	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_t *pSrc32, *pDst32;

	size = 0x2000;
	pSrc32 = (uint32_t*)0xBFC00000;
	pDst32 = (uint32_t*)calloc(size,sizeof(uint32_t));
	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_t*)calloc(size,sizeof(uint32_t));
	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_t int_part, result;
	uint16_t pi_res[200] = {0};
	uint16_t 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_t)(result - int_part);
		carry = sum % PI_SCALE;
		k++;
	}

	for (k=0; k < sizeof(pi_ref)/sizeof(uint16_t); 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_t *ram8 = NULL;
	uint16_t *ram16 = NULL;
	uint32_t *ram32 = NULL;
	uint32_t volatile *pFlashIO = (uint32_t*)SYS_FLASH_IO;
	uint32_t volatile *pFlashMem = (uint32_t*)SYS_FLASH_MEM;
	uint32_t volatile *pROM = (uint32_t*)0xBFC00000;
	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_moffs_1 = (uint32_t*)SYS_VGA_FB_BACK;
	uint32_t volatile *pVGA_const_color = (uint32_t*)SYS_VGA_BLIT_COLOR;


	uint32_t *pSrc32, *pDst32;
	uint32_t start, end;
	uint32_t color_cycle[12] = {0x000000FF, 0x00808080, 0x0000FFFF, 0x00808080, 0x0000FF00, 0x00808080, 0x00FFFF00, 0x00808080, 0x00FF0000, 0x00808080, 0x00FF00FF, 0x00808080};

	// ----------------------------------------------------------
	// 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_t)i;

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

	PrintBuffer8((uint8_t*)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_t)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_t*)calloc(TEST_SIZE/4,sizeof(uint32_t));
	pDst32 = (uint32_t*)calloc(TEST_SIZE/4,sizeof(uint32_t));

	// Memtest BEGIN
	printf("Dump first 16 bytes of flash blocks\n");
	for (i=0; i < FLASH_SIZE; i += 0x40000)
		memdump((uint8_t*)(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_t*)pDst32;
	printf("Zeroize SDRAM (memset)...");
	start = clock();
	memset(ram8, 0, TEST_SIZE);
	end = clock();
	printf("done (%.2f MByte/s)\n", (double)TEST_SIZE/(1024*1024*(double)(end-start)/CLOCKS_PER_SEC));

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

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

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

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

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

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

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

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

	printf("Verify SDRAM (8-Bit access)...");
	start = clock();
	for (i=TEST_SIZE-1; i >= 0; i--)
		if (ram8[i] != (uint8_t)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*1024*(double)(end-start)/CLOCKS_PER_SEC));

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

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

	printf("Verify SDRAM (16-Bit access)...");
	start = clock();
	for (i=TEST_SIZE/2-1; i >= 0; i--)
		if (ram16[i] != (uint16_t)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*1024*(double)(end-start)/CLOCKS_PER_SEC));


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

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

	printf("Verify SDRAM (32-Bit access)...");
	start = clock();
	for (i=TEST_SIZE/4-1; i >= 0; i--)
	{
		if (ram32[i] != (uint32_t)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*1024*(double)(end-start)/CLOCKS_PER_SEC));

	printf("\n");
	printf("Small data test (size = %d kByte)\n", SMALL_TEST_SIZE/1024);
	printf("Write SDRAM (32-Bit access)...");
	ram32 = (uint32_t*)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_t)i;

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

	// -------------------------------------------------------------------
	// 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_t)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*1024*(double)(end-start)/CLOCKS_PER_SEC));

	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*1024*(double)(end-start)/CLOCKS_PER_SEC));
	}
	for (i=1024; i < 4*65536; i *= 2)
	{
		printf("_GFX_copy32() (%d kByte)...", i/1024);
		start = clock();
		for (j=0; j < TEST_SIZE/i; j++)
			_GFX_copy32(pDst32, pSrc32, i/4);

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

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

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

#if 1
	*pVGA_moffs_0 = (uint32_t)pSrc32;
	*pVGA_moffs_1 = (uint32_t)pSrc32;
	*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_FIN_INTEN;
	*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_EMPTY_INTEN;

	for (cnt=0; cnt < 3; cnt++)
	{
		*pVGA_ctrl &= ~(SYS_VGA_BIT_BLIT_OP_CONSTCOL | SYS_VGA_BIT_MSTEN);
		fprintf(stderr, "Blitter copy %d bytes (scan disabled)...", TEST_SIZE/4);
		*pVGA_src0 = (uint32_t)pSrc32;
		*pVGA_dst = (uint32_t)pDst32;
		*pVGA_dimx_src0 = (uint32_t)0;
		*pVGA_dimx_dst = (uint32_t)0;
		*pVGA_nx = (uint32_t)TEST_SIZE/4;
		*pVGA_ny = (uint32_t)1;

		start = clock();
		for (i=0; i < 10; i++)
		{
			while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
		}
		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
		end = clock();
		fprintf(stderr, "done (%.2f MByte/s)\n", (double)(i*TEST_SIZE)/(1024*1024*(double)(end-start)/CLOCKS_PER_SEC));

		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_OP_CONSTCOL;
		fprintf(stderr, "Blitter memset %d bytes (scan disabled)...", TEST_SIZE/4);
		*pVGA_src0 = (uint32_t)pSrc32;
		*pVGA_dst = (uint32_t)pDst32;
		*pVGA_dimx_src0 = (uint32_t)0;
		*pVGA_dimx_dst = (uint32_t)0;
		*pVGA_nx = (uint32_t)TEST_SIZE/4;
		*pVGA_ny = (uint32_t)1;

		start = clock();
		for (i=0; i < 10; i++)
		{
			*pVGA_const_color = color_cycle[i%12];
			while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
		}
		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
		end = clock();
		fprintf(stderr, "done (%.2f MByte/s)\n", (double)(i*TEST_SIZE)/(1024*1024*(double)(end-start)/CLOCKS_PER_SEC));

		fprintf(stderr, "Blitter copy %d bytes (scan enabled)...", TEST_SIZE/4);
		*pVGA_ctrl |= SYS_VGA_BIT_MSTEN;
		*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
		sleep(500);
		*pVGA_src0 = (uint32_t)pSrc32;
		*pVGA_dst = (uint32_t)pDst32;
		*pVGA_dimx_src0 = (uint32_t)0;
		*pVGA_dimx_dst = (uint32_t)0;
		*pVGA_nx = (uint32_t)TEST_SIZE/4;
		*pVGA_ny = (uint32_t)1;

		start = clock();
		for (i=0; i < 10; i++)
		{
			while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
		}
		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
		end = clock();
		fprintf(stderr, "done (%.2f MByte/s)\n", (double)(i*TEST_SIZE)/(1024*1024*(double)(end-start)/CLOCKS_PER_SEC));

		*pVGA_ctrl |= SYS_VGA_BIT_BLIT_OP_CONSTCOL;
		fprintf(stderr, "Blitter memset %d bytes (scan enabled)...", TEST_SIZE/4);
		*pVGA_src0 = (uint32_t)pSrc32;
		*pVGA_dst = (uint32_t)pDst32;
		*pVGA_dimx_src0 = (uint32_t)0;
		*pVGA_dimx_dst = (uint32_t)0;
		*pVGA_nx = (uint32_t)TEST_SIZE/4;
		*pVGA_ny = (uint32_t)1;

		start = clock();
		for (i=0; i < 10; i++)
		{
			*pVGA_const_color = color_cycle[i%12];
			while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
			*pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
		}
		while (*pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
		end = clock();
		fprintf(stderr, "done (%.2f MByte/s)\n", (double)(i*TEST_SIZE)/(1024*1024*(double)(end-start)/CLOCKS_PER_SEC));
		fprintf(stderr, "\n");

	}

	*pVGA_ctrl &= ~SYS_VGA_BIT_MSTEN;
	*pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_FIN_INTEN;
	interrupt_disable(5);
#endif

	size = 0x2000;
	memcpy(pDst32, (uint32_t*)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");

	DCACHE_invalidate_all();
	printf("pFlashIO  = %08X\n", pFlashIO);
	printf("pFlashMem = %08X\n", pFlashMem);
	printf("Verify FLASH(IO) <=> FLASH(MEM)...");
	for (i=0; i < FLASH_SIZE/4; i++)
	{
		if (pFlashIO[i] != pFlashMem[i])
		{
			printf("\nfailed at offset = %08X", i << 2);
			printf("\nfound %08X <=> %08X", pFlashIO[i], pFlashMem[i]);
		}
	}
	printf("\n");

	if (i != FLASH_SIZE/4)
	{
		printf("failed\n");
		memdump((uint8_t*)&pFlashIO[i], 0, 16, 256);
		memdump((uint8_t*)&pFlashMem[i], 0, 16, 256);
	}
	else
	{
		printf("passed\n");
	}

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

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

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

int main (void)
{
	int result, i, j, cnt, size;
	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 *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;
	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;
	uint32_t *pPic;


	pPic = (uint32_t*)malloc(800*600*sizeof(uint32_t));
	memcpy(pPic, pFlashPicture, 800*600*sizeof(uint32_t));

	g_cnt = 1;
	int screen_fd = open("VGA-0", 0, 0);
	gpio_init(&gpio_if, SYS_GPIO_0_BASE);

	*pVGA_cgcol = 0x00FFFFFF;

	*pTim0_cnt = 0;
	*pTim1_cnt = 0;
	*pTim0_cmp = 0x40000;
	*pTim1_cmp = 100000000;
	*pTim_stat = (1 << 2) | (1 << 0);
	*pTim_ctrl = (3 << 2);

//	*pUART_baud = 1;

	setbuf(stdout, NULL);
	PrintCPUinfo();

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

	interrupt_register(3, handler3);
	fprintf(stderr, "UART interrupt registered.\n");

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

	calibrate_delay();

	printf("\n");

	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');
		}
	}
	srand(clock());
	gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, uncached_cachemiss_bug());

	Test16_MemTest();

	printf("pf  = %08X\n", (uint32_t)pf());
	*pf() = 0x1234;
	printf("*pf = %08X\n", (uint32_t)*pf());

	box_create(&box, 32, 32);
	GFX_init(&gfx);

	start = clock();
	for (i=0; i < 10000; i++)
	{
		GFX_box_draw_direct(&gfx, &box, (uint32_t)rand()%800, (uint32_t)rand()%600, (uint32_t)rand());
	}

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

	GFX_frame(&gfx, GFX_FRAME_SYNC);
	GFX_set_background(&gfx, pPic, 800, 600, 1, 1);
	posx = posy = 0;
	GFX_show(&gfx);

	interrupt_enable(7);
	*pTim_ctrl |= 3;
	interrupt_enable(3);
	*pUART_stat |= (1 << 6);

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

		gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, cnt);

		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++;

//		printf("V=%.17e\n", pow((double)cnt-1, (double)cnt));
	}
	printf("Failed with error %8.8X\n", result);

	return 1;

}