#include <stdio.h>
#include <stdlib.h>
#include "libsys/utils.h"

#define	NO_ERROR		0x00000000
#define	ERROR			0x80000000
#define	IS_ERROR(e)		((e & ERROR) == ERROR)

#define	TEST10_ERROR	(ERROR | 0x10)
#define	TEST11_ERROR	(ERROR | 0x11)
#define	TEST12_ERROR	(ERROR | 0x12)

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

int StatusRegister_read(void)
{
	__asm
	(
		".set	noreorder\n"
		"mfc0   $2, $12\n"
		".set	reorder\n"

	);
}

int RevisionRegister_read(void)
{
	__asm
	(
		".set	noreorder\n"
		"mfc0   $2, $15\n"
		".set	reorder\n"

	);
}

void PrintCPUinfo(void)
{
	int result, rev_id;

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

	// Print Status register
	result = StatusRegister_read();
	sputs("Status  : ");
	print_word(result);
	sputs("\r\n");

	// Print Revision
	result = RevisionRegister_read();
	rev_id = (result >> 8) & 0xFF;
	sputs("CPU type: ");
	if ((rev_id > 0) && (rev_id < 0x07))
	{
		sputs(cpu_type_str[rev_id]);
		sputs(" Rev.");
		rev_id = result & 0xFF;
		print_byte((char)rev_id);
	}
	else
		sputs("Unknown");

	sputs("\r\n");

}

int Test10_LoadStore()
{
	int buf[32*sizeof(int)], i, result, data, err;
	volatile int *pPtr;

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

	return err;

}


#define	NUM_ELEMENTS	20
#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 ^ i) - 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;

	srand(0x19701031);
	for (i=0; i < 100; i++)
	{
		for (j=1; j < 1000; 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;

			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 main (void)
{
	int result, i, j, cnt;
	volatile int *pReg = (int*)0x80000000;
	PrintCPUinfo();

	cnt = 0;
	while (1)
	{
		*pReg = cnt & 0x3FFFFFFF;

		result = Test10_LoadStore();
		if (IS_ERROR(result))
			break;

		result = Test11_AddSub();
		if (IS_ERROR(result))
			break;

		result = Test12_MulDiv();
		if (IS_ERROR(result))
			break;

//		printf("Iteration %d: Passed\r\n", cnt++);
		sputs("Iteration ");
		print_word(cnt++);
		sputs(": Passed\r\n");

	}
	*pReg = 0x40000000 | cnt;
	sputs("Failed with error ");
	print_word(result);
	sputs(" \r\n");

	return 1;

}