//
// Cforall Version 1.0.0 Copyright (C) 2016 University of Waterloo
//
// The contents of this file are covered under the licence agreement in the
// file "LICENCE" distributed with Cforall.
//
// kernel.c --
//
// Author           : Thierry Delisle
// Created On       : Tue Jan 17 12:27:26 2017
// Last Modified By : Peter A. Buhr
// Last Modified On : Fri Jul 21 22:33:18 2017
// Update Count     : 2
//

#include "libhdr.h"

//C Includes
#include <stddef.h>
extern "C" {
#include <stdio.h>
#include <fenv.h>
#include <sys/resource.h>
#include <signal.h>
#include <unistd.h>
}

//CFA Includes
#include "kernel_private.h"
#include "preemption.h"
#include "startup.h"

//Private includes
#define __CFA_INVOKE_PRIVATE__
#include "invoke.h"

//Start and stop routine for the kernel, declared first to make sure they run first
void kernel_startup(void)  __attribute__(( constructor( STARTUP_PRIORITY_KERNEL ) ));
void kernel_shutdown(void) __attribute__(( destructor ( STARTUP_PRIORITY_KERNEL ) ));

//-----------------------------------------------------------------------------
// Kernel storage
KERNEL_STORAGE(cluster,           mainCluster);
KERNEL_STORAGE(processor,         mainProcessor);
KERNEL_STORAGE(processorCtx_t,    mainProcessorCtx);
KERNEL_STORAGE(thread_desc,       mainThread);
KERNEL_STORAGE(machine_context_t, mainThreadCtx);

cluster *     mainCluster;
processor *   mainProcessor;
thread_desc * mainThread;

//-----------------------------------------------------------------------------
// Global state

thread_local coroutine_desc * volatile this_coroutine;
thread_local thread_desc *    volatile this_thread;
thread_local processor *      volatile this_processor;

volatile thread_local bool preemption_in_progress = 0;
volatile thread_local unsigned short disable_preempt_count = 1;

//-----------------------------------------------------------------------------
// Main thread construction
struct current_stack_info_t {
	machine_context_t ctx;
	unsigned int size;		// size of stack
	void *base;				// base of stack
	void *storage;			// pointer to stack
	void *limit;			// stack grows towards stack limit
	void *context;			// address of cfa_context_t
	void *top;				// address of top of storage
};

void ?{}( current_stack_info_t & this ) {
	CtxGet( this.ctx );
	this.base = this.ctx.FP;
	this.storage = this.ctx.SP;

	rlimit r;
	getrlimit( RLIMIT_STACK, &r);
	this.size = r.rlim_cur;

	this.limit = (void *)(((intptr_t)this.base) - this.size);
	this.context = &storage_mainThreadCtx;
	this.top = this.base;
}

void ?{}( coStack_t & this, current_stack_info_t * info) {
	this.size = info->size;
	this.storage = info->storage;
	this.limit = info->limit;
	this.base = info->base;
	this.context = info->context;
	this.top = info->top;
	this.userStack = true;
}

void ?{}( coroutine_desc & this, current_stack_info_t * info) {
	(this.stack){ info };
	this.name = "Main Thread";
	this.errno_ = 0;
	this.state = Start;
}

void ?{}( thread_desc & this, current_stack_info_t * info) {
	(this.cor){ info };
}

//-----------------------------------------------------------------------------
// Processor coroutine
void ?{}(processorCtx_t & this, processor * proc) {
	(this.__cor){ "Processor" };
	this.proc = proc;
	proc->runner = &this;
}

void ?{}(processorCtx_t & this, processor * proc, current_stack_info_t * info) {
	(this.__cor){ info };
	this.proc = proc;
	proc->runner = &this;
}

void ?{}(processor & this) {
	this{ mainCluster };
}

void ?{}(processor & this, cluster * cltr) {
	this.cltr = cltr;
	(this.terminated){ 0 };
	this.do_terminate = false;
	this.preemption_alarm = NULL;
	this.pending_preemption = false;

	start( &this );
}

void ?{}(processor & this, cluster * cltr, processorCtx_t & runner) {
	this.cltr = cltr;
	(this.terminated){ 0 };
	this.do_terminate = false;
	this.preemption_alarm = NULL;
	this.pending_preemption = false;
	this.kernel_thread = pthread_self();

	this.runner = &runner;
	LIB_DEBUG_PRINT_SAFE("Kernel : constructing main processor context %p\n", &runner);
	runner{ &this };
}

void ^?{}(processor & this) {
	if( ! this.do_terminate ) {
		LIB_DEBUG_PRINT_SAFE("Kernel : core %p signaling termination\n", &this);
		this.do_terminate = true;
		P( &this.terminated );
		pthread_join( this.kernel_thread, NULL );
	}
}

void ?{}(cluster & this) {
	( this.ready_queue ){};
	( this.ready_queue_lock ){};

	this.preemption = default_preemption();
}

void ^?{}(cluster & this) {

}

//=============================================================================================
// Kernel Scheduling logic
//=============================================================================================
//Main of the processor contexts
void main(processorCtx_t & runner) {
	processor * this = runner.proc;

	LIB_DEBUG_PRINT_SAFE("Kernel : core %p starting\n", this);

	{
		// Setup preemption data
		preemption_scope scope = { this };

		LIB_DEBUG_PRINT_SAFE("Kernel : core %p started\n", this);

		thread_desc * readyThread = NULL;
		for( unsigned int spin_count = 0; ! this->do_terminate; spin_count++ )
		{
			readyThread = nextThread( this->cltr );

			if(readyThread)
			{
				verify( disable_preempt_count > 0 );

				runThread(this, readyThread);

				verify( disable_preempt_count > 0 );

				//Some actions need to be taken from the kernel
				finishRunning(this);

				spin_count = 0;
			}
			else
			{
				spin(this, &spin_count);
			}
		}

		LIB_DEBUG_PRINT_SAFE("Kernel : core %p stopping\n", this);
	}

	V( &this->terminated );

	LIB_DEBUG_PRINT_SAFE("Kernel : core %p terminated\n", this);
}

// runThread runs a thread by context switching
// from the processor coroutine to the target thread
void runThread(processor * this, thread_desc * dst) {
	coroutine_desc * proc_cor = get_coroutine(*this->runner);
	coroutine_desc * thrd_cor = get_coroutine(dst);

	//Reset the terminating actions here
	this->finish.action_code = No_Action;

	//Update global state
	this_thread = dst;

	// Context Switch to the thread
	ThreadCtxSwitch(proc_cor, thrd_cor);
	// when ThreadCtxSwitch returns we are back in the processor coroutine
}

// Once a thread has finished running, some of
// its final actions must be executed from the kernel
void finishRunning(processor * this) {
	if( this->finish.action_code == Release ) {
		unlock( this->finish.lock );
	}
	else if( this->finish.action_code == Schedule ) {
		ScheduleThread( this->finish.thrd );
	}
	else if( this->finish.action_code == Release_Schedule ) {
		unlock( this->finish.lock );
		ScheduleThread( this->finish.thrd );
	}
	else if( this->finish.action_code == Release_Multi ) {
		for(int i = 0; i < this->finish.lock_count; i++) {
			unlock( this->finish.locks[i] );
		}
	}
	else if( this->finish.action_code == Release_Multi_Schedule ) {
		for(int i = 0; i < this->finish.lock_count; i++) {
			unlock( this->finish.locks[i] );
		}
		for(int i = 0; i < this->finish.thrd_count; i++) {
			ScheduleThread( this->finish.thrds[i] );
		}
	}
	else {
		assert(this->finish.action_code == No_Action);
	}
}

// Handles spinning logic
// TODO : find some strategy to put cores to sleep after some time
void spin(processor * this, unsigned int * spin_count) {
	(*spin_count)++;
}

// Context invoker for processors
// This is the entry point for processors (kernel threads)
// It effectively constructs a coroutine by stealing the pthread stack
void * CtxInvokeProcessor(void * arg) {
	processor * proc = (processor *) arg;
	this_processor = proc;
	this_coroutine = NULL;
	this_thread = NULL;
	disable_preempt_count = 1;
	// SKULLDUGGERY: We want to create a context for the processor coroutine
	// which is needed for the 2-step context switch. However, there is no reason
	// to waste the perfectly valid stack create by pthread.
	current_stack_info_t info;
	machine_context_t ctx;
	info.context = &ctx;
	processorCtx_t proc_cor_storage = { proc, &info };

	LIB_DEBUG_PRINT_SAFE("Coroutine : created stack %p\n", proc_cor_storage.__cor.stack.base);

	//Set global state
	this_coroutine = &proc->runner->__cor;
	this_thread = NULL;

	//We now have a proper context from which to schedule threads
	LIB_DEBUG_PRINT_SAFE("Kernel : core %p created (%p, %p)\n", proc, proc->runner, &ctx);

	// SKULLDUGGERY: Since the coroutine doesn't have its own stack, we can't
	// resume it to start it like it normally would, it will just context switch
	// back to here. Instead directly call the main since we already are on the
	// appropriate stack.
	proc_cor_storage.__cor.state = Active;
	main( proc_cor_storage );
	proc_cor_storage.__cor.state = Halted;

	// Main routine of the core returned, the core is now fully terminated
	LIB_DEBUG_PRINT_SAFE("Kernel : core %p main ended (%p)\n", proc, proc->runner);

	return NULL;
}

void start(processor * this) {
	LIB_DEBUG_PRINT_SAFE("Kernel : Starting core %p\n", this);

	pthread_create( &this->kernel_thread, NULL, CtxInvokeProcessor, (void*)this );

	LIB_DEBUG_PRINT_SAFE("Kernel : core %p started\n", this);
}

//-----------------------------------------------------------------------------
// Scheduler routines
void ScheduleThread( thread_desc * thrd ) {
	// if( !thrd ) return;
	verify( thrd );
	verify( thrd->cor.state != Halted );

	verify( disable_preempt_count > 0 );

	verifyf( thrd->next == NULL, "Expected null got %p", thrd->next );

	lock(   &this_processor->cltr->ready_queue_lock DEBUG_CTX2 );
	append( &this_processor->cltr->ready_queue, thrd );
	unlock( &this_processor->cltr->ready_queue_lock );

	verify( disable_preempt_count > 0 );
}

thread_desc * nextThread(cluster * this) {
	verify( disable_preempt_count > 0 );
	lock( &this->ready_queue_lock DEBUG_CTX2 );
	thread_desc * head = pop_head( &this->ready_queue );
	unlock( &this->ready_queue_lock );
	verify( disable_preempt_count > 0 );
	return head;
}

void BlockInternal() {
	disable_interrupts();
	verify( disable_preempt_count > 0 );
	suspend();
	verify( disable_preempt_count > 0 );
	enable_interrupts( DEBUG_CTX );
}

void BlockInternal( spinlock * lock ) {
	disable_interrupts();
	this_processor->finish.action_code = Release;
	this_processor->finish.lock = lock;

	verify( disable_preempt_count > 0 );
	suspend();
	verify( disable_preempt_count > 0 );

	enable_interrupts( DEBUG_CTX );
}

void BlockInternal( thread_desc * thrd ) {
	assert(thrd);
	disable_interrupts();
	assert( thrd->cor.state != Halted );
	this_processor->finish.action_code = Schedule;
	this_processor->finish.thrd = thrd;

	verify( disable_preempt_count > 0 );
	suspend();
	verify( disable_preempt_count > 0 );

	enable_interrupts( DEBUG_CTX );
}

void BlockInternal( spinlock * lock, thread_desc * thrd ) {
	assert(thrd);
	disable_interrupts();
	this_processor->finish.action_code = Release_Schedule;
	this_processor->finish.lock = lock;
	this_processor->finish.thrd = thrd;

	verify( disable_preempt_count > 0 );
	suspend();
	verify( disable_preempt_count > 0 );

	enable_interrupts( DEBUG_CTX );
}

void BlockInternal(spinlock ** locks, unsigned short count) {
	disable_interrupts();
	this_processor->finish.action_code = Release_Multi;
	this_processor->finish.locks = locks;
	this_processor->finish.lock_count = count;

	verify( disable_preempt_count > 0 );
	suspend();
	verify( disable_preempt_count > 0 );

	enable_interrupts( DEBUG_CTX );
}

void BlockInternal(spinlock ** locks, unsigned short lock_count, thread_desc ** thrds, unsigned short thrd_count) {
	disable_interrupts();
	this_processor->finish.action_code = Release_Multi_Schedule;
	this_processor->finish.locks = locks;
	this_processor->finish.lock_count = lock_count;
	this_processor->finish.thrds = thrds;
	this_processor->finish.thrd_count = thrd_count;

	verify( disable_preempt_count > 0 );
	suspend();
	verify( disable_preempt_count > 0 );

	enable_interrupts( DEBUG_CTX );
}

void LeaveThread(spinlock * lock, thread_desc * thrd) {
	verify( disable_preempt_count > 0 );
	this_processor->finish.action_code = thrd ? Release_Schedule : Release;
	this_processor->finish.lock = lock;
	this_processor->finish.thrd = thrd;

	suspend();
}

//=============================================================================================
// Kernel Setup logic
//=============================================================================================
//-----------------------------------------------------------------------------
// Kernel boot procedures
void kernel_startup(void) {
	LIB_DEBUG_PRINT_SAFE("Kernel : Starting\n");

	// Start by initializing the main thread
	// SKULLDUGGERY: the mainThread steals the process main thread
	// which will then be scheduled by the mainProcessor normally
	mainThread = (thread_desc *)&storage_mainThread;
	current_stack_info_t info;
	(*mainThread){ &info };

	LIB_DEBUG_PRINT_SAFE("Kernel : Main thread ready\n");

	// Initialize the main cluster
	mainCluster = (cluster *)&storage_mainCluster;
	(*mainCluster){};

	LIB_DEBUG_PRINT_SAFE("Kernel : main cluster ready\n");

	// Initialize the main processor and the main processor ctx
	// (the coroutine that contains the processing control flow)
	mainProcessor = (processor *)&storage_mainProcessor;
	(*mainProcessor){ mainCluster, *(processorCtx_t *)&storage_mainProcessorCtx };

	//initialize the global state variables
	this_processor = mainProcessor;
	this_thread = mainThread;
	this_coroutine = &mainThread->cor;

	// Enable preemption
	kernel_start_preemption();

	// Add the main thread to the ready queue
	// once resume is called on mainProcessor->runner the mainThread needs to be scheduled like any normal thread
	ScheduleThread(mainThread);

	// SKULLDUGGERY: Force a context switch to the main processor to set the main thread's context to the current UNIX
	// context. Hence, the main thread does not begin through CtxInvokeThread, like all other threads. The trick here is that
	// mainThread is on the ready queue when this call is made.
	resume( *mainProcessor->runner );



	// THE SYSTEM IS NOW COMPLETELY RUNNING
	LIB_DEBUG_PRINT_SAFE("Kernel : Started\n--------------------------------------------------\n\n");

	enable_interrupts( DEBUG_CTX );
}

void kernel_shutdown(void) {
	LIB_DEBUG_PRINT_SAFE("\n--------------------------------------------------\nKernel : Shutting down\n");

	disable_interrupts();

	// SKULLDUGGERY: Notify the mainProcessor it needs to terminates.
	// When its coroutine terminates, it return control to the mainThread
	// which is currently here
	mainProcessor->do_terminate = true;
	suspend();

	// THE SYSTEM IS NOW COMPLETELY STOPPED

	// Disable preemption
	kernel_stop_preemption();

	// Destroy the main processor and its context in reverse order of construction
	// These were manually constructed so we need manually destroy them
	^(*mainProcessor->runner){};
	^(mainProcessor){};

	// Final step, destroy the main thread since it is no longer needed
	// Since we provided a stack to this taxk it will not destroy anything
	^(mainThread){};

	LIB_DEBUG_PRINT_SAFE("Kernel : Shutdown complete\n");
}

static spinlock kernel_abort_lock;
static spinlock kernel_debug_lock;
static bool kernel_abort_called = false;

void * kernel_abort    (void) __attribute__ ((__nothrow__)) {
	// abort cannot be recursively entered by the same or different processors because all signal handlers return when
	// the globalAbort flag is true.
	lock( &kernel_abort_lock DEBUG_CTX2 );

	// first task to abort ?
	if ( !kernel_abort_called ) {			// not first task to abort ?
		kernel_abort_called = true;
		unlock( &kernel_abort_lock );
	}
	else {
		unlock( &kernel_abort_lock );

		sigset_t mask;
		sigemptyset( &mask );
		sigaddset( &mask, SIGALRM );			// block SIGALRM signals
		sigaddset( &mask, SIGUSR1 );			// block SIGUSR1 signals
		sigsuspend( &mask );				// block the processor to prevent further damage during abort
		_exit( EXIT_FAILURE );				// if processor unblocks before it is killed, terminate it
	}

	return this_thread;
}

void kernel_abort_msg( void * kernel_data, char * abort_text, int abort_text_size ) {
	thread_desc * thrd = kernel_data;

	int len = snprintf( abort_text, abort_text_size, "Error occurred while executing task %.256s (%p)", thrd->cor.name, thrd );
	__lib_debug_write( STDERR_FILENO, abort_text, len );

	if ( thrd != this_coroutine ) {
		len = snprintf( abort_text, abort_text_size, " in coroutine %.256s (%p).\n", this_coroutine->name, this_coroutine );
		__lib_debug_write( STDERR_FILENO, abort_text, len );
	}
	else {
		__lib_debug_write( STDERR_FILENO, ".\n", 2 );
	}
}

extern "C" {
	void __lib_debug_acquire() {
		lock( &kernel_debug_lock DEBUG_CTX2 );
	}

	void __lib_debug_release() {
		unlock( &kernel_debug_lock );
	}
}

//=============================================================================================
// Kernel Utilities
//=============================================================================================
//-----------------------------------------------------------------------------
// Locks
void ?{}( spinlock & this ) {
	this.lock = 0;
}
void ^?{}( spinlock & this ) {

}

bool try_lock( spinlock * this DEBUG_CTX_PARAM2 ) {
	return this->lock == 0 && __sync_lock_test_and_set_4( &this->lock, 1 ) == 0;
}

void lock( spinlock * this DEBUG_CTX_PARAM2 ) {
	for ( unsigned int i = 1;; i += 1 ) {
		if ( this->lock == 0 && __sync_lock_test_and_set_4( &this->lock, 1 ) == 0 ) { break; }
	}
	LIB_DEBUG_DO(
		this->prev_name = caller;
		this->prev_thrd = this_thread;
	)
}

void lock_yield( spinlock * this DEBUG_CTX_PARAM2 ) {
	for ( unsigned int i = 1;; i += 1 ) {
		if ( this->lock == 0 && __sync_lock_test_and_set_4( &this->lock, 1 ) == 0 ) { break; }
		yield();
	}
	LIB_DEBUG_DO(
		this->prev_name = caller;
		this->prev_thrd = this_thread;
	)
}


void unlock( spinlock * this ) {
	__sync_lock_release_4( &this->lock );
}

void  ?{}( semaphore & this, int count = 1 ) {
	(this.lock){};
	this.count = count;
	(this.waiting){};
}
void ^?{}(semaphore & this) {}

void P(semaphore * this) {
	lock( &this->lock DEBUG_CTX2 );
	this->count -= 1;
	if ( this->count < 0 ) {
		// queue current task
		append( &this->waiting, (thread_desc *)this_thread );

		// atomically release spin lock and block
		BlockInternal( &this->lock );
	}
	else {
	    unlock( &this->lock );
	}
}

void V(semaphore * this) {
	thread_desc * thrd = NULL;
	lock( &this->lock DEBUG_CTX2 );
	this->count += 1;
	if ( this->count <= 0 ) {
		// remove task at head of waiting list
		thrd = pop_head( &this->waiting );
	}

	unlock( &this->lock );

	// make new owner
	WakeThread( thrd );
}

//-----------------------------------------------------------------------------
// Queues
void ?{}( __thread_queue_t & this ) {
	this.head = NULL;
	this.tail = &this.head;
}

void append( __thread_queue_t * this, thread_desc * t ) {
	verify(this->tail != NULL);
	*this->tail = t;
	this->tail = &t->next;
}

thread_desc * pop_head( __thread_queue_t * this ) {
	thread_desc * head = this->head;
	if( head ) {
		this->head = head->next;
		if( !head->next ) {
			this->tail = &this->head;
		}
		head->next = NULL;
	}
	return head;
}

thread_desc * remove( __thread_queue_t * this, thread_desc ** it ) {
	thread_desc * thrd = *it;
	verify( thrd );

	(*it) = thrd->next;

	if( this->tail == &thrd->next ) {
		this->tail = it;
	}

	thrd->next = NULL;

	verify( (this->head == NULL) == (&this->head == this->tail) );
	verify( *this->tail == NULL );
	return thrd;
}

void ?{}( __condition_stack_t & this ) {
	this.top = NULL;
}

void push( __condition_stack_t * this, __condition_criterion_t * t ) {
	verify( !t->next );
	t->next = this->top;
	this->top = t;
}

__condition_criterion_t * pop( __condition_stack_t * this ) {
	__condition_criterion_t * top = this->top;
	if( top ) {
		this->top = top->next;
		top->next = NULL;
	}
	return top;
}

// Local Variables: //
// mode: c //
// tab-width: 4 //
// End: //