/* 
 * Copyright 2010 Stefan Lankes, Chair for Operating Systems,
 *                               RWTH Aachen University
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * This file is part of MetalSVM. 
 */

#include <metalsvm/stddef.h>
#include <metalsvm/stdio.h>
#include <metalsvm/time.h>
#include <metalsvm/tasks.h>
#include <metalsvm/semaphore.h>
#include <metalsvm/mailbox.h>
#include <metalsvm/syscall.h>
#include <metalsvm/vma.h>
#include <metalsvm/page.h>
#ifdef CONFIG_LWIP
#include <lwip/opt.h>
#endif
#ifdef CONFIG_ROCKCREEK
#include <asm/icc.h>
#include <asm/RCCE.h>
#include <asm/RCCE_lib.h>
#include <asm/iRCCE.h>
#include <asm/iRCCE_lib.h>
#include <asm/SCC_API.h>
#include <asm/svm.h>
#endif

#include "tests.h"

int laplace(void* arg);
int jacobi(void* arg);
int memory(void* arg);
void echo_init(void);
void netio_init(void);

#if defined(START_CONSUMER_PRODUCER) || defined(START_CHIEFTEST)
static sem_t 		consuming, producing;
static mailbox_int32_t	mbox;
static int 		val = 0;

static int consumer(void* arg)
{
	int 	i, m = 0;

	for(i=0; i<5; i++) {
		sem_wait(&consuming, 0);
		kprintf("Consumer got %d\n", val);
		val = 0;
		sem_post(&producing);
	}

	for(i=0; i<5; i++) {
		mailbox_int32_fetch(&mbox, &m, 0);
		kprintf("Got mail %d\n", m);
	}

	return 0;
}

static int producer(void* arg)
{
	int 	i;
	int 	mail[5] = {1, 2, 3, 4, 5};

	for(i=0; i<5; i++) {
		sem_wait(&producing, 0);
		kprintf("Produce value: current val %d\n", val);
		val = 42;
		sem_post(&consuming);
	}

	for(i=0; i<5; i++) {
		//kprintf("Send mail %d\n", mail[i]);
		mailbox_int32_post(&mbox, mail[i]);
	}

	return 0;
}

static int producer_consumer(void)
{
	tid_t id1, id2, ret;
	int result1, result2, result;

	create_kernel_task(&id1, producer, NULL, NORMAL_PRIO);
	create_kernel_task(&id2, consumer, NULL, NORMAL_PRIO);

	ret = wait(&result);
	if (ret == id1) result1 = result;
	else result2 = result;

	ret = wait(&result);
	if (ret == id1) result1 = result;
	else result2 = result;

	return result1 || result2;
}
#endif

#if defined(START_FOO) || defined(START_JOIN_TEST) || defined(START_CHIEFTEST)
static int foo(void* arg)
{
	int i;

	if (!arg)
		return 0;

	for(i=0; i<5; i++) {
		kprintf("Message from core %u: %s\n", CORE_ID, (char*) arg);
		sleep(1);
	}

	return 42;
}
#endif

#if (defined(START_MAIL_PING) || defined(START_CHIEFTEST)) && defined(CONFIG_ROCKCREEK)
static int mail_ping(void* arg) {
	int i;

	//for(i=0; i<5; ++i)
	//	icc_mail_ping();
	for(i=0; i<5; ++i) 
		icc_mail_ping_irq(); 
	//icc_mail_ping_jitter(); 
	//icc_irq_ping();      
	//icc_mail_datarates();
	//icc_halt();

	return 0;
}
#endif

#ifdef START_MAIL_NOISE
static int mail_noise(void*arg) {
	icc_mail_noise(); // generate noise in the mesh
	return 0;
}
#endif

#if (defined(START_SVM_TEST) || defined(START_CHIEFTEST)) && defined(CONFIG_ROCKCREEK)

/* N has to be multiple of UEs */

#define N	1024
//#define N 	512
//#define N	128

//#define SVM_TYPE      SVM_STRONG
#define SVM_TYPE        SVM_LAZYRELEASE

volatile static int* A[N];
volatile static int* B[N];
volatile static int* C[N];

#if 0
#define GET_B(i, j)	B[i][j]
#else
#define GET_B(i, j)	B[j][i]
#endif

static int svm_test(void *arg)
{
	uint64_t start, end;
	uint32_t i, j, k;

	uint32_t svm_flags;

	int my_ue, num_ues;
	register int tmp;

	kputs("Start SVM test...\n");

	RCCE_barrier(&RCCE_COMM_WORLD);
	my_ue = RCCE_ue();
	num_ues = RCCE_num_ues();

#if 1
	if (!my_ue) {
		// allocate and initialize SVM region
		A[0] = (int*) kmalloc(3*N*N*sizeof(int));
		if (!A[0])
			return 1; // Let the others hang in the barrier. People will notice that things went wrong.
		memset((void*) A[0], 0x00, 3*N*N*sizeof(int));

		// initialize matrices
		for(i=0; i<N; i++) {
			A[i] = A[0] + i*N;
			B[i] = A[0] + (i*N + N*N);
			C[i] = A[0] + (i*N + 2*N*N);
		}

		for(i=0; i<N; i++) {
			A[i][i] = 1;
			for(j=0; j<N; j++)
				GET_B(i,j) = i+j;
		}

		kputs("Start sequential calculation...\n");

		start = rdtsc();
		start = rdtsc();

		// start calculation
		for(i=0; i<N; i++) {
			for(j=0; j<N; j++) {
				tmp = C[i][j];
				for(k=0; k<N; k++)
					C[i][j] += A[i][k] * GET_B(k,j);
				C[i][j] = tmp;
			}
		}

		end = rdtsc();

		kprintf("Calculation time (seq): %llu ms (%llu ticks)\n", (end-start)/(1000ULL*get_cpu_frequency()), end-start);
		kfree(A[0], 3*N*N*sizeof(int));
	}

	RCCE_barrier(&RCCE_COMM_WORLD);
#endif

	// allocate and initialize SVM region

	svm_flags = SVM_TYPE;
	if (svm_flags & SVM_LAZYRELEASE)
		kputs("Use Lazy Release consistency!\n");	
	else
		kputs("Use Strong Release consistency!\n");

	A[0] = (int*) svm_malloc(3*N*N*sizeof(int), svm_flags);
	if (!A[0]) 
		return 1;

#if 1
	if (!my_ue) 
		memset((void*) A[0], 0x00, 3*N*N*sizeof(int));
#endif

	// initialize matrices
	for(i=0; i<N; i++) {
		A[i] = A[0] + i*N;
		B[i] = A[0] + (i*N + N*N);
		C[i] = A[0] + (i*N + 2*N*N);
	}

#if 1
	// distriubute page frames over all MC via affinity on first touch
	for(i=my_ue*(N/num_ues); i<(my_ue+1)*(N/num_ues); i++) {
		memset(A[i], 0x00, N*sizeof(int));
		memset(B[i], 0x00, N*sizeof(int));
		memset(C[i], 0x00, N*sizeof(int));
	}
#endif

	svm_barrier(svm_flags);

	if (!my_ue) {
		for(i=0; i<N; i++) {
			A[i][i] = 1;
			for(j=0; j<N; j++)
				GET_B(i,j) = i+j;
		}
	}

	svm_barrier(svm_flags);

	kputs("Start parallel calculation...\n");

	start = rdtsc();
	start = rdtsc();

	// Now, we need only read access on A and B
	//change_page_permissions((size_t) A[0], (size_t) (A[0]+2*N*N), VMA_CACHEABLE|VMA_READ);
	//svm_barrier(SVM_TYPE);

	// start calculation
	for(i=my_ue*(N/num_ues); i<(my_ue+1)*(N/num_ues); i++) {
		for(j=0; j<N; j++) {
			tmp = C[i][j];
			for(k=0; k<N; k++)
				tmp += A[i][k] * GET_B(k,j);
			C[i][j] = tmp;
		}
	}

	svm_barrier(SVM_TYPE);
	
	end = rdtsc();

	kputs("Check results...\n");

	uint32_t err = 0; // Be paranoid for a test case. return != 0 if calculation was not correct.
	if (!my_ue) {
		svm_invalidate();
		for(i=0; (i<N) && (err < 32); i++) {
			for(j=0; (j<N) && (err < 32); j++) {
				if (C[i][j] != i+j) {
					err++;
					kprintf("Wrong value at C[%u][%u] = %u, B[%u][%u] = %u = %u\n", i, j, C[i][j], i, j, GET_B(i,j), B[i][j]);
				}
			}
		}
	}

	svm_barrier(SVM_TYPE);

	kprintf("Calculation time (par): %llu ms (%llu ticks)\n", (end-start)/(1000ULL*get_cpu_frequency()), end-start);

	svm_free((void*) A[0], 3*N*sizeof(int));

	svm_statistics();

	return err;
}
#endif

#if (defined(START_SVM_BENCH) || defined(START_CHIEFTEST)) && defined(CONFIG_ROCKCREEK)
static int svm_bench(void *arg)
{
	volatile uint32_t* array = NULL;
	uint64_t start, end;
	uint32_t i;
	const uint32_t size = N*N*sizeof(uint32_t);
	const uint32_t svm_flags = SVM_TYPE;

	if (RCCE_IAM > 1)
		return -1;

	if (svm_flags & SVM_LAZYRELEASE)
		kputs("Use Lazy Release consistency!\n");
	else
		kputs("Use Strong Release consistency!\n");
	svm_barrier(svm_flags);

	start = rdtsc();
	start = rdtsc();
	array = (volatile uint32_t*) svm_malloc(size, svm_flags);
	if (!array)
		return -1;
	end = rdtsc();

	if (BUILTIN_EXPECT(!array, 0)) {
		kprintf("Out of memory\n");
			return -1;
	}

	kprintf("Time to allocate %u Bytes: %llu usec (%llu ticks)\n", size, (end-start)/get_cpu_frequency(), end-start);

	svm_barrier(svm_flags);
	if (!RCCE_IAM) {
		start = rdtsc();
		// The pages are created by touching them for the first time.
		for(i=0; i<size/sizeof(uint32_t); i+=PAGE_SIZE/sizeof(uint32_t))
			array[i] = 0;
		end = rdtsc();

		kprintf("Time to create %u page frames: %llu usec (%llu ticks)\n", size >> PAGE_SHIFT, (end-start)/get_cpu_frequency(), end-start);
	}

	svm_barrier(svm_flags);
	if (RCCE_IAM) {
		start = rdtsc();
		// Touching these pages from another core means moving them 
		// to another owner as well as mapping them in the other core's
		// memory space.
		for(i=0; i<size/sizeof(uint32_t); i+=PAGE_SIZE/sizeof(uint32_t))
			array[i] = 1;
		end = rdtsc();

		kprintf("Time to map %u page frames: %llu usec (%llu ticks)\n", size >> PAGE_SHIFT, (end-start)/get_cpu_frequency(), end-start);
	}

	svm_barrier(svm_flags);
	if (!RCCE_IAM) {
		start = rdtsc();
		// Touching the pages from the first core again only involves
		// moving the pages without the need to map them again.
		for(i=0; i<size/sizeof(uint32_t); i+=PAGE_SIZE/sizeof(uint32_t))
			array[i] = 0;
		end = rdtsc();

		kprintf("Time to get access permissions of %u page frames: %llu usec (%llu ticks)\n", size >> PAGE_SHIFT, (end-start)/get_cpu_frequency(), end-start);
	}

	svm_barrier(svm_flags);
	start = rdtsc();
	change_page_permissions((size_t) array, size, VMA_CACHEABLE|VMA_READ);
	end = rdtsc();
	kprintf("Time to change access permissions of %u page frames: %llu usec (%llu ticks)\n", size >> PAGE_SHIFT, (end-start)/get_cpu_frequency(), end-start);

	svm_barrier(svm_flags);
	svm_free((void*) array, N*N*sizeof(uint32_t));

	svm_statistics();

	return 0;
}

static int testcase_svm_bench(void)
{
	if (RCCE_IAM > 1) {
		int i;
		for (i=0; i < 6; i++) 
			// Need as many barriers for the other cores as they occur in the benchmark.
			svm_barrier(SVM_TYPE);
		return 0;
	}
	return svm_bench(NULL);
}
#endif

#if defined(START_JOIN_TEST) || defined(START_CHIEFTEST)
static int join_test(void* arg)
{
	tid_t 	id, ret;
	int 	result = -1234;

	create_kernel_task(&id, foo, "Hello from foo2", HIGH_PRIO-1);

	kprintf("Wait for child %u\n", id);
	do {
		ret = wait(&result);
	} while(ret != id);

	kprintf("Child %u finished: result = %d\n", id, result);

	return result != 42;
}
#endif

#ifdef START_PI
#ifndef M_PI
#define M_PI	3.14159265358979323846264338327950288   /* pi */
#endif

static int pi(void* arg)
{
	double x, sum, step;
	int i, num_steps = 100000000;

	sum = 0.0;
	step = 1.0 / (double)num_steps;

	for (i = 0; i < num_steps; i++) {
		x = (i + 0.5) * step;
		sum += 4.0 / (1.0 + x * x);
	}

	x = M_PI - sum * step;
	x *= 10000.0;

	kprintf("Distance to PI = %u/10000\n", (uint32_t) x);

	return 0;
}
#endif

#ifdef START_MEASURE_CTX_SWITCH
#define REPS 10000

volatile uint64_t t1, t2;
volatile int stop = !!0;
volatile int sid = 0;

static int measure_ctx_switch(void* arg)
{
	int id = !!(int)arg;
	int oid = !id;
	uint64_t freq  = get_cpu_frequency() *1000 *1000;
	uint64_t diff, min = (uint64_t)-1, max = 0, avg = 0;
	int i;
	uint32_t a=0,b,c,d;

	// Size of a timeslice in ticks
	uint64_t timeslice = freq / TIMER_FREQ;
  
  	kprintf("ID: %d, ", id);
	kprintf("Measuring SW task switching.\n");

	for (i=0; i < REPS && stop == 0; i++) {
		while(id == sid && stop == 0) {
			t2 = rdtsc();
			// Calling rdtsc that often it is better
			// to serialize the calls in the pipeline.
			cpuid(0,&a,&b,&c,&d);
		}

		cpuid(0,&a,&b,&c,&d);
		diff = rdtsc() -t2;

		// The last measurement is garbage
		if (stop) break;
		// The first ones are garbage, too
		if (i < 5) goto next_try;
		if (diff >= timeslice) {
			i--;
			goto next_try;
		}

		kprintf("%i: diff= %llu, i= %i\n", id, diff, i);
		if (diff > max) max = diff;
		if (diff < min) min = diff;
		avg += diff;

next_try:
		sid = id;
	}
	avg /= i-5;

	stop = 1;

	kprintf("maximum gap: %llu ticks\n", max);
	kprintf("minimum gap: %llu ticks\n", min);
	kprintf("average gap: %llu ticks\n", avg);
	kprintf("Timeslice size: %llu ticks\n", timeslice);

	return 0;
 }
#endif

#ifdef START_CHIEFTEST

struct testcase_t {
	tid_t tid;
	int retval;
	char* userspace_argv[10];
	entry_point_t kernelspace_ep;
	char* testcase_name;
};

#define USERSPACE_TC(execpath, name) {0, 0, {execpath, NULL}, NULL, name}
#define KERNELSPACE_TC(execpointer, name) {0, 0, {NULL}, execpointer, name}

struct testcase_t testcases[] = {
//	USERSPACE_TC("/bin/jacobi", "Jacobi serial user space app"),
	USERSPACE_TC("/bin/tests", "Tests user space app"),
	//USERSPACE_TC("/bin/hello", "Hello userspace app"),
	KERNELSPACE_TC(producer_consumer, "Producer consumer kernel space test"),
	KERNELSPACE_TC(join_test, "Join kernel space test"),
#ifdef CONFIG_ROCKCREEK
	//KERNELSPACE_TC(svm_test, "SVM Test kernel space test"),
//	KERNELSPACE_TC(testcase_svm_bench, "SVM Bench kernel space test"),
	KERNELSPACE_TC(mail_ping, "Mail Ping kernel space test"),
//	KERNELSPACE_TC(jacobi, "Jacobi kernel space test"),
//	KERNELSPACE_TC(netio_init, "NetIO kernel space test")
#endif
};

static int chiefmastertest(void)
{
	int tests = sizeof(testcases) / sizeof(struct testcase_t);
	int i, j;
	int result;
	tid_t ret;
	struct testcase_t* current;

	int retval[tests];
	int tmpval;
#ifdef CONFIG_ROCKCREEK
	int *global_retval = NULL;

	kprintf("Starting chief master test: %d test cases on %d cores.\n", tests, RCCE_NP);
#else
	kprintf("Starting chief master test: %d test cases.\n", tests);
#endif

	for (i = 0; i < tests; i++) {
		current = &testcases[i];

#ifdef CONFIG_ROCKCREEK
		/* TODO: This blocks forever. Why? */
		//RCCE_barrier(&RCCE_COMM_WORLD);
#endif
		kprintf("Starting testcase: %s\n", current->testcase_name);

		if (current->kernelspace_ep == NULL &&
				current->userspace_argv[0] != NULL) {
			create_user_task(&current->tid, current->userspace_argv[0],
					current->userspace_argv);
		} else if (current->kernelspace_ep != NULL &&
				current->userspace_argv[0] == NULL) {
			create_kernel_task(&current->tid, current->kernelspace_ep, 
					NULL, NORMAL_PRIO);
		} else {
			kprintf("Invalid test case struct #%d!\n", i);
			continue;
		}

		do {
			ret = wait(&result);
		} while(ret != current->tid);

		kprintf("Testcase %s returned %d %s\n", current->testcase_name, result, result ? ":(" : ":)");

		retval[i] = result;
	}

#ifdef CONFIG_ROCKCREEK
	if (RCCE_IAM == 0) { // Master
		global_retval = kmalloc(tests * RCCE_NP);
		memcpy(global_retval, retval, sizeof(retval));

		for (i=1; i < RCCE_NP; i++)
			iRCCE_recv((void*)&global_retval[tests * i], sizeof(*global_retval) * tests, i);
	}
	else { // worker cores
		iRCCE_send((void*)retval, tests * sizeof(retval[0]), 0);
	}
#endif
	kprintf("======================================================\n");
	kprintf("Master Chief test case results:\n");
	for (i = 0; i < tests; i++) {
		current = &testcases[i];
#ifdef CONFIG_ROCKCREEK
		tmpval = 0;
		for (j=0; j < RCCE_NP; j++)
			tmpval |= global_retval[i + j*tests];
#else
		tmpval = retval[i];
#endif

		kprintf("%s: ", current->testcase_name);
		if (tmpval) {
			pushfg(COL_RED);
#ifdef CONFIG_ROCKCREEK
			if (RCCE_IAM == 0) {
				kprintf("Bad :( [on cores: ");
				for (j=0; j < RCCE_NP; j++)
					if (global_retval[i + j*tests])
						kprintf("%d ", j);
				kprintf("]");
			}
			else 
#endif
				kprintf("Bad :(");
		} else {
			pushfg(COL_GREEN);
			kprintf("Pass :)");
#ifdef CONFIG_ROCKCREEK
			if (RCCE_IAM == 0) kprintf(" on all cores");
#endif
		}
		popfg();
		kprintf("\n");
	}
	kprintf("======================================================\n");

#ifdef CONFIG_ROCKCREEK
		kfree(global_retval, tests * RCCE_NP);
#endif

	return 0;
}

#endif // START_CHIEFTEST

int test_init(void)
{
#ifdef START_HELLO
	char* hello_argv[] = {"/bin/hello", NULL};
#endif
#ifdef START_TESTS
	char* tests_argv[] = {"/bin/tests", NULL};
#endif
#ifdef START_JACOBI
	char* jacobi_argv[] = {"/bin/jacobi", NULL};
#endif
#ifdef START_MMNIF_TEST
	char* server_argv[] = {"/bin/server", "6789", NULL};
	char* client_argv[] = {"/bin/client", "192.168.0.1", "6789", NULL};
#endif

#ifdef START_CHIEFTEST
	create_kernel_task(NULL, chiefmastertest, NULL, NORMAL_PRIO);
#endif

#ifdef START_ECHO
        echo_init();
#endif
#ifdef START_NETIO
	netio_init();
#endif
#if defined(START_CONSUMER_PRODUCER) || defined(START_CHIEFTEST)
	sem_init(&producing, 1);
	sem_init(&consuming, 0);
	mailbox_int32_init(&mbox);
#endif
#ifdef START_CONSUMER_PRODUCER
	create_kernel_task(NULL, producer_consumer, NULL, NORMAL_PRIO);
#endif
#ifdef  START_MEASURE_CTX_SWITCH
	create_kernel_task(NULL, measure_ctx_switch, (int)0, NORMAL_PRIO);
	create_kernel_task(NULL, measure_ctx_switch, (int)1, NORMAL_PRIO);
#endif
#ifdef START_FOO
	create_kernel_task(NULL, foo, "Hello from foo1", NORMAL_PRIO);
	//create_kernel_task_on_core(NULL, foo, "Hello from foo2", NORMAL_PRIO, 1);
#endif
#ifdef START_JOIN_TEST
	create_kernel_task(NULL, join_test, NULL, NORMAL_PRIO);
#endif
#ifdef START_MAIL_PING
	create_kernel_task(NULL, mail_ping, NULL, NORMAL_PRIO);
#endif
#ifdef START_MAIL_NOISE
	create_kernel_task(NULL, mail_noise, NULL, NORMAL_PRIO);
#endif
#ifdef START_SVM_TEST
	create_kernel_task(NULL, svm_test, NULL, NORMAL_PRIO);
#endif
#ifdef START_SVM_BENCH
	create_kernel_task(NULL, svm_bench, NULL, NORMAL_PRIO);
#endif
#ifdef START_PI
	create_kernel_task(NULL, pi, NULL, NORMAL_PRIO);
#endif
#ifdef START_KERNEL_LAPLACE
	create_kernel_task(NULL, laplace, NULL, NORMAL_PRIO);
#endif
#ifdef START_KERNEL_JACOBI
	create_kernel_task(NULL, jacobi, NULL, NORMAL_PRIO);
#endif
#ifdef START_HELLO
	create_user_task(NULL, "/bin/hello", hello_argv);
#endif
#ifdef START_TESTS
	create_user_task(NULL, "/bin/tests", tests_argv);
#endif
#ifdef START_JACOBI
	create_user_task(NULL, "/bin/jacobi", jacobi_argv);
	//create_user_task_on_core(NULL, "/bin/jacobi", jacobi_argv, 1);
#endif
#if defined(START_MMNIF_TEST) && defined(CONFIG_LWIP) && LWIP_SOCKET
	if (RCCE_IAM == 0) {
		kprintf("Start /bin/server...\n");
		create_user_task(NULL, "/bin/server", server_argv);
	} else {
		sleep(5);
		kprintf("Start /bin/client...\n");
		create_user_task(NULL, "/bin/client", client_argv);
	}
#endif
#ifdef START_MEMORY
	create_kernel_task(NULL, memory, NULL, NORMAL_PRIO);
#endif

	return 0;
}