/* 
 * 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 COND */

#include <metalsvm/stdio.h>
#include <metalsvm/errno.h>
#include <metalsvm/processor.h>
#include <metalsvm/errno.h>
#include <asm/io.h>
#include <asm/irqflags.h>
#include <asm/irq.h>
#ifdef CONFIG_ROCKCREEK
#include <asm/RCCE.h>
#include <asm/RCCE_lib.h>
#include <asm/iRCCE.h>
#include <asm/SCC_API.h>
#include <asm/icc.h>
#include <asm/svm.h>
#include <asm/limits.h>

#define IRQ_STATUS				0xD000
#define IRQ_MASK				0xD200
#define IRQ_RESET				0xD400
#define IRQ_REQUEST				0xD600
#define IRQ_CONFIG				0xD800

#include <net/rckemac.h>

bootinfo_t* bootinfo = (bootinfo_t*) SCC_BOOTINFO;

/* PSE bit for Pentium+ equals MPE (message buffer enable) flag in RCK! So, use it to create _PAGE_MPB symbol... */
#define _CR4_MPE 0x00000800

/* maximal number of SCC's cores */
#define MAX_SCC_CORES	(NUM_ROWS*NUM_COLS*NUM_CORES)

/*
 * This is the modified MPB program, which is part of the RCCE distribution (src/mpb.c).
 *
 * This function clears the local MPB and resets the test&set register.
 */
static int scc_clear(void)
{
	int tmp, x, y, z, offset;

	// Initialize API
	InitAPI(0);
 
	// Find out who I am...
	tmp=ReadConfigReg(CRB_OWN+MYTILEID); 
	x=(tmp>>3) & 0x0f; // bits 06:03
	y=(tmp>>7) & 0x0f; // bits 10:07
	z=(tmp   ) & 0x07; // bits 02:00

	// Allocate Message Passing Buffer
	t_vcharp MPB;
	MPBalloc(&MPB, x, y, z, 1); 
	if (!MPB) {
		kprintf("Unable to allocate MPB for core %d of Tile x=%d, y= %d! Exiting.\n", z, x, y);
		return 255;
	}

	// zap own MPB
	for (offset=0; offset < 0x2000; offset+=8)
		*(volatile unsigned long long int*)(MPB+offset) = 0;

	// Clear test&set register write. Next read-access will read "1" (lock granted).
	SetConfigReg(CRB_ADDR(x,y)+((z)?LOCK1:LOCK0), 1); 

	// frees Message Passing Buffer
	MPBunalloc(&MPB);

	return 0;
}

static inline void icc_mail_check_tag(iRCCE_MAIL_HEADER* mail) {
	char* recv_buffer;

	if(BUILTIN_EXPECT(!mail, 0))
		return;

	switch( mail->tag ) {
	case iRCCE_ANYLENGTH:
		recv_buffer = (char*)kmalloc( mail->size );
		iRCCE_irecv(recv_buffer, mail->size, mail->source, NULL);
		break;
	case PING_REQ:
		iRCCE_mail_send(0, PING_RESP, 0, NULL, mail->source);
		break;
	case SVM_REQ:
		svm_emit_page(((size_t*) mail->payload)[1], ((size_t*) mail->payload)[0]);
		break;
	case SVM_RESP:
		break;
	case NOISE:
//		kprintf( "XXX " );
	default:
//		kprintf( "icc_mail_check_tag: uknown tag id %d\n", mail->tag );
		break;
	}
}

static void icc_handler(struct state *s)
{
	// reset appropriate bit in the core configuration register
	iRCCE_MAIL_HEADER* header = NULL;
	int tmp, source;
	uint32_t status_low, status_high, status;
	static int z = -1;
	volatile static uint32_t* status_addr;
	volatile static uint32_t* reset_addr;

	if (z < 0) {
		z = Z_PID(RC_MY_COREID);
		status_addr = (volatile uint32_t*) (FPGA_BASE + IRQ_STATUS + RC_MY_COREID * 8);
		reset_addr = (volatile uint32_t*) (FPGA_BASE + IRQ_RESET + RC_MY_COREID * 8);
	}

	status_low = status_addr[0];
	status_high = status_addr[1];

#ifdef CONFIG_LWIP
	rckemacif_handler(s, status_low);
#endif

	if ((status_low >> 6) || status_high)
	{
		/* determine interrupt sources */
		status = status_low;
		status >>= 6;	// shift emac bits

		for (source=0; status!=0; status >>= 1, ++source) {
			if (((status & 0x1) != 0) &&  (RC_RCCEID[source] >= 0))
				iRCCE_mail_check(RC_RCCEID[source]);
		}

		for (source=26, status=status_high; status!=0; status >>= 1, ++source) {
			if (((status & 0x1) != 0) &&  (RC_RCCEID[source] >= 0))
				iRCCE_mail_check(RC_RCCEID[source]);
		}
	} else iRCCE_mail_check(iRCCE_MAILBOX_ALL);

	tmp=ReadConfigReg(CRB_OWN + (z==0 ? GLCFG0 : GLCFG1));
	tmp &= ~2;
	SetConfigReg(CRB_OWN + (z==0 ? GLCFG0 : GLCFG1), tmp);

	/* Reset */
	if (status_low)
		reset_addr[0] = status_low;
	if (status_high)
		reset_addr[1] = status_high;

	/* empty mail queue */
	while( iRCCE_mail_recv(&header) == iRCCE_SUCCESS ) {
		icc_mail_check_tag(header);
		iRCCE_mail_release(&header);
	}
}

int icc_init(void)
{
	int i, z, tmp;
	uint64_t start, end, ticks, freq = 533;
	uint32_t cr4;
	uint32_t msg;

	kputs("Initialize Rock Creek!\n");

	/* Enable Messagepassing in CR4 */
	cr4 = read_cr4();
	cr4 = cr4 | _CR4_MPE;
	write_cr4(cr4);

	kprintf("address of the initrd: 0x%x\n", bootinfo->addr);
	kprintf("size of the initrd: %d\n", bootinfo->size);
	kprintf("rcce argc = %d\n", bootinfo->argc);
	for(i=0; i<bootinfo->argc; i++)
		kprintf("rcce argv[%d] = %s\n", i, bootinfo->argv[i]);

	if (bootinfo->argc >= 3)
		freq = atoi(bootinfo->argv[2]);

	kputs("Reset SCC!\n");
	scc_clear();
	kputs("Wait some time...\n");
	mb();
	start = rdtsc();
	do {
		mb();
		end = rdtsc();
		ticks = end > start ? end - start : start - end;
	} while(ticks*TIMER_FREQ < 300ULL*freq*1000000ULL);

	if (RCCE_init(&bootinfo->argc, &bootinfo->argv) != RCCE_SUCCESS)
		return -ENODEV;
	if (iRCCE_init() != iRCCE_SUCCESS)
		return -ENODEV;

	// enable additional outputs	
	//RCCE_debug_set(RCCE_DEBUG_ALL);

	kprintf("Got rank %d of %d ranks\n", RCCE_IAM, RCCE_NP);

	RCCE_barrier(&RCCE_COMM_WORLD);

	kputs("RCCE test...\t");
	if (!RCCE_IAM)
		msg = 0x4711;
	if ((RCCE_bcast((char*) &msg, sizeof(msg), 0, RCCE_COMM_WORLD) == RCCE_SUCCESS) && (msg == 0x4711))
		kprintf("successfull! (0x%x)\n", msg);
	else
		kprintf("failed! (0x%x)\n", msg);

	// reset INTR/LINT0 flag
	z = Z_PID(RC_MY_COREID);
	tmp=ReadConfigReg(CRB_OWN + (z==0 ? GLCFG0 : GLCFG1));
	tmp &= ~(1 << GLCFG_XINTR_BIT);
	SetConfigReg(CRB_OWN + (z==0 ? GLCFG0 : GLCFG1), tmp);

#if 0
	// disable L2 cache
	z = Z_PID(RC_COREID[my_ue]);
	tmp=ReadConfigReg(CRB_OWN + (z==0 ? L2CFG0 : L2CFG1));
	tmp |= (1 << L2CFG_WAYDISABLE_BIT);
	SetConfigReg(CRB_OWN + (z==0 ? L2CFG0 : L2CFG1), tmp);
	kprintf("set L2CFG to 0x%x\n", (uint32_t) tmp);
#endif

	tmp=ReadConfigReg(CRB_OWN + (z==0 ? L2CFG0 : L2CFG1));
	kputs("In the config registers is the L2 cache ");
	if (tmp & (1 << L2CFG_WAYDISABLE_BIT))
		kputs("disabled!\n");
	else
		kputs("enabled!\n");

	kputs("In CR0 is caching ");
	if (read_cr0() & (1 << 30))
		kputs("disabled!\n");
	else
		kputs("enabled!\n");

	kputs("In CR0 is writethrough caching ");
	if (read_cr0() & (1 << 29))
		kputs("enabled!\n");
	else
		kputs("disabled!\n");

	// set interrupt handler (LINT0)
	irq_install_handler(124, icc_handler);

	// unmask interrupts
	volatile uint32_t* irq_mask = (volatile uint32_t*)(FPGA_BASE + IRQ_MASK + RC_COREID[RCCE_IAM]*8);
	irq_mask[0] = 0x3F;
	irq_mask[1] = 0x00;

	// set remote interrupts to LINT 0
	volatile uint32_t* irq_config = (volatile uint32_t*)(FPGA_BASE + IRQ_CONFIG + RC_COREID[RCCE_IAM]*4);
	irq_config[0] = 0x00;

	kprintf( "irq_mask = 0x%x, 0x%x\n", irq_mask[0], irq_mask[1]);
	kprintf( "irq_config = 0x%x\n", irq_config[0]);
	kputs("Now, the SCC is initialized!\n");

	return 0;
}

int icc_halt(void)
{
	icc_mail_check();
	//NOP1;
	
	HALT;

	return 0;
}

#define ROUNDS 20000
#define CORE_A	RC_RCCEID[0] 	// sender
#define CORE_B	RC_RCCEID[30]	// receiver 

int icc_send_gic_irq(int core_num) {
	volatile uint32_t* irq_request = (volatile uint32_t*)(FPGA_BASE+IRQ_REQUEST+RC_MY_COREID*8);
	uint32_t bit_pos;
	
	if (BUILTIN_EXPECT((core_num < 0)  || (core_num > 48), 0))
		return -EINVAL;

	// determine bit position and set according bit
	if (RC_COREID[core_num] < 32) {
		bit_pos = (1 << RC_COREID[core_num]);
		irq_request[0] = bit_pos;
	} else {
		bit_pos = (1 << (RC_COREID[core_num]-32));
		irq_request[1] = bit_pos;
	}
	
	return 0;
}

int icc_mail_ping(void)
{	
	uint32_t flags;
	uint64_t timer = 0;
	int i;
	int res;
	iRCCE_MAIL_HEADER* recv_header = NULL;

	/* leave function if not participating in pingpong */
	if( (RCCE_IAM != CORE_A) && (RCCE_IAM != CORE_B) ) return -1;

	kprintf( "my rank = %d\n", RCCE_IAM);
	kprintf( "Hello from mail_ping ... \n" );
	kprintf( "rounds = %d\n", ROUNDS );


	// disable interrupts
	flags = irq_nested_disable();
	
	for( i=0; i<ROUNDS+1; ++i ) {
		/* senders part */
		if( RCCE_IAM == CORE_A ) {
			/* send ping request */ 
			iRCCE_mail_send(0, PING_REQ, 0, NULL, CORE_B);

			/* wait for response */
			do {
				res = iRCCE_mail_check(iRCCE_MAILBOX_ALL); //CORE_B);
			} while( res != iRCCE_SUCCESS );

			/* release mail */
			iRCCE_mail_recv(&recv_header);
			iRCCE_mail_release(&recv_header);
		}
		/* receivers part */
		else {
			/* wait for request */
			do {
				res = iRCCE_mail_check(iRCCE_MAILBOX_ALL); //CORE_A);
			} while( res != iRCCE_SUCCESS );

			/* check mail */
			res = iRCCE_mail_recv(&recv_header);
			icc_mail_check_tag(recv_header);

			/* release mail */
			iRCCE_mail_release(&recv_header);
		}
		
		/* start timer in first round */
		if( i == 0 ) timer = rdtsc();

	}

	/* stop timer */
	timer = rdtsc() - timer;


	if( RCCE_IAM == CORE_A ) {
		kprintf( "timer = %ld\n", timer );
		kprintf( "mail_pingpong needs in average %d ns (%d ticks)!\n",
		timer*1000/(2*ROUNDS*get_cpu_frequency()), timer/(2*ROUNDS) );
	}

	irq_nested_enable(flags);

	return 0;
}

int icc_mail_ping_irq(void)
{
	kprintf( "Hello from mail_ping_irq ... \n" );
	/* return if not core A */
	if( RCCE_IAM != CORE_A ) return 0;
	
	uint32_t flags;
	uint64_t timer = 0;
	int i;
	int res;
	iRCCE_MAIL_HEADER* recv_header = NULL;

	kprintf( "my rank = %d\n", RCCE_IAM );
	kprintf( "rem_rank = %d\n", CORE_B );	
	kprintf( "rounds = %d\n", ROUNDS );
	
	// disable interrupts
	flags = irq_nested_disable();
	
	for( i=0; i<ROUNDS+1; ++i ) {
		/* send ping request */ 
		iRCCE_mail_send(0, PING_REQ, 0, NULL, CORE_B);

		/* send interrupt */
		icc_send_gic_irq(CORE_B);

		/* wait for response */
		do {
			res = iRCCE_mail_check(CORE_B);
		} while( res != iRCCE_SUCCESS );
		
		iRCCE_mail_recv(&recv_header);
		iRCCE_mail_release(&recv_header);
	
		/* start timer in first round */
		if( i == 0 ) timer = rdtsc();
		
	}

	/* stop timer */
	timer = rdtsc() - timer;

	kprintf( "timer = %d\n", timer );
	kprintf( "mail_pingpong needs in average %d nsec (%d ticks)!\n",
		timer*1000/(2*ROUNDS*get_cpu_frequency()), timer/(2*ROUNDS) );
	
	irq_nested_enable(flags);
	return 0;
}

int icc_mail_ping_jitter(void)
{
	kprintf( "Hello from jitter_test ... \n" );
	/* return if not core A */
	if( RCCE_IAM != CORE_A ) return 0;

	uint32_t flags;
	uint64_t timer = 0;
	uint64_t max = 0;
	uint64_t min = ULONG_MAX;
	uint64_t sum = 0;

	int i;
	int res;
	iRCCE_MAIL_HEADER* recv_header = NULL;

	kprintf( "my_rank = %d\n", RCCE_IAM );
	kprintf( "rem_rank = %d\n", CORE_B );   
	kprintf( "rounds = %d\n", ROUNDS );

	// disable interrupts
	flags = irq_nested_disable();

	for( i=0; i<ROUNDS+1; ++i ) {
		/* start timer */
		timer = rdtsc();

		/* send ping request */ 
		iRCCE_mail_send(0, PING_REQ, 0, NULL, CORE_B);

		/* send interrupt */
		icc_send_gic_irq(CORE_B);

		/* wait for response */
		do {
			res = iRCCE_mail_check(CORE_B);
		} while( res != iRCCE_SUCCESS );

		iRCCE_mail_recv(&recv_header);
		iRCCE_mail_release(&recv_header);

		/* stop timer and update eval values */
		timer = rdtsc() - timer;
		if( i > 0 ) {
			max = ( max < timer )? timer : max;
			min = ( min > timer )? timer : min;
			sum += timer;
		}
	}

	kprintf( "Average was: %d nsec\n", sum*1000/(2*ROUNDS*533) );
	kprintf( "Maximum was: %d nsec\n", max*1000/(2*533) );
	kprintf( "Minimum was: %d nsec\n", min*1000/(2*533) );
	kprintf( "Jitter  was: %d nsec\n", (max-min)*1000/(2*533) ); 

	irq_nested_enable(flags);

	return 0;
}

#undef _IRQ_NOISE_
#define NOISE_PRIO 1

int icc_mail_noise(void) {
	int i, j, res;
	int num_ranks = RCCE_num_ues();
	int count = 0;
	iRCCE_MAIL_HEADER* recv_mail = NULL;

	/* timer vars */
	uint64_t timer;
	uint64_t tmr;
	uint64_t tmr_send = 0;
	uint64_t tmr_recv = 0;
	uint64_t tmr_release = 0;
	uint64_t tmr_chck = 0;

	kprintf( "my_ue = %d\n", RCCE_IAM );

	// leave function if not participating
	if( (RCCE_IAM == CORE_A) || (RCCE_IAM == CORE_B) ) {
		kprintf( "mail_noise: leaving" );
		return -1;
	}  


	kprintf( "Hello from icc_mail_noise: my_ue = %d\n", RCCE_IAM );
	kprintf( "num_ues = %d\n", num_ranks );

	timer = rdtsc();

	for( i=0; i<40000; ++i ) {	
		if( !(i%1000) ) kprintf( "%d ", i );
		tmr = rdtsc();
		iRCCE_mail_check(iRCCE_MAILBOX_ALL);
		tmr = rdtsc() - tmr;
		tmr_chck += tmr;

		/* send a mail to each UE */
		for( j=0; j<num_ranks; ++j ) {
			if( (j == CORE_A)  || (j == CORE_B) ) continue;

			/* send noise mail */
			tmr = rdtsc();
			iRCCE_mail_send(0, NOISE, NOISE_PRIO, NULL, j);
			tmr = rdtsc() - tmr;
			tmr_send += tmr;

			tmr = rdtsc();
			res = iRCCE_mail_recv(&recv_mail);
			tmr = rdtsc() - tmr;
			tmr_recv += tmr;
		
			if( res == iRCCE_SUCCESS ) {
				icc_mail_check_tag(recv_mail);
				tmr = rdtsc();
				iRCCE_mail_release(&recv_mail);
				tmr = rdtsc() - tmr;
				tmr_release += tmr;
				count++;
			}
		}
	}

	do {
		tmr = rdtsc();
		iRCCE_mail_check(iRCCE_MAILBOX_ALL);
		tmr = rdtsc() - tmr;
		tmr_chck += tmr;

		tmr = rdtsc();
		res = iRCCE_mail_recv(&recv_mail);
		tmr = rdtsc() - tmr;
		tmr_recv += tmr;

		if( res == iRCCE_SUCCESS ) {
			icc_mail_check_tag(recv_mail);
			tmr = rdtsc();
			iRCCE_mail_release(&recv_mail);
			tmr = rdtsc() - tmr;
			tmr_release += tmr;
			count++;
		}
	} while( res == iRCCE_SUCCESS );

	timer = rdtsc() - timer;

	kprintf( "Count = %d\n", count );

	kprintf( "Time: %d ms\n", timer/(1000*get_cpu_frequency()) );
	kprintf( "Time in send: %d ms\n", tmr_send/(1000*get_cpu_frequency()) );
	kprintf( "Time in recv: %d ms\n", tmr_recv/(1000*get_cpu_frequency()) );
	kprintf( "Time in chck: %d ms\n", tmr_chck/(1000*get_cpu_frequency()) );
	kprintf( "Time in release: %d ms\n", tmr_release/(1000*get_cpu_frequency()) );

	kprintf( "XXX XXX XXX" );
	return 0;
}

/* 
 * Routine to check mailboxes. 
 */

void icc_mail_check(void)
{
	iRCCE_MAIL_HEADER* header = NULL;
	uint32_t flags;

	/* disable interrupts */	
	flags = irq_nested_disable();

	iRCCE_mail_check(iRCCE_MAILBOX_ALL);

	/* empty mail queue */
	while( iRCCE_mail_recv(&header) == iRCCE_SUCCESS ) {
		icc_mail_check_tag(header);
		iRCCE_mail_release( &header );
	}

	/* enable interrupts */
	irq_nested_enable(flags);
}

//uint64_t check_ticks = 0;
//uint64_t recv_ticks = 0;

void icc_wait(int tag)
{
	iRCCE_MAIL_HEADER* header = NULL;
        uint32_t flags;
	//uint64_t start;
	
	/* disable interrupts */
	flags = irq_nested_disable();

retry:
	//start = rdtsc();
	iRCCE_mail_check(iRCCE_MAILBOX_ALL);
	//check_ticks += rdtsc() - start;

	//start = rdtsc();
	/* empty mail queue */
	while(iRCCE_mail_recv(&header) == iRCCE_SUCCESS ) {
		icc_mail_check_tag(header);
		if (header->tag == tag) {
			iRCCE_mail_release( &header );
			goto out;
		} else iRCCE_mail_release( &header );
	}
	//recv_ticks += rdtsc() - start;

	goto retry;

out:
	//recv_ticks += rdtsc() - start;
	/* enable interrupts */
	irq_nested_enable(flags);
}
#endif