///////////////////////////////////////////////////////////////////////////////////
// File     : kernel_init.c
// Date     : 26/05/2012
// Authors  : alain greiner & mohamed karaoui
// Copyright (c) UPMC-LIP6
////////////////////////////////////////////////////////////////////////////////////
// The kernel_init.c files is part of the GIET-VM nano-kernel.
// It contains the kernel entry point for the last step of system initialisation. 
// All procs in this phase have their MMU activated, and are running in parallel.
// - each processor updates its own scheduler, to replace the ISR index 
//   by the actual ISR virtual address, and set its own entry in the 
//   kernel _schedulers_paddr[] array.
// - each processor initialises the SP, SR, PTPR, EPC registers, and starts 
//   its private Timer, before jumping to the user code with an eret. 
////////////////////////////////////////////////////////////////////////////////////

#include <common.h>
#include <irq_handler.h>
#include <ctx_handler.h>
#include <sys_handler.h>
#include <mapping_info.h>
#include <giet_config.h>
#include <mips32_registers.h>
#include <irq_handler.h>
#include <vm_handler.h>
#include <hwr_mapping.h>
#include <mwmr_channel.h>
#include <barrier.h>
#include <drivers.h>

///////////////////////////////////////////////////////////////////////////////////
// Kernel Global variables
///////////////////////////////////////////////////////////////////////////////////

__attribute__((section (".kdata"))) 
page_table_t* _ptabs_paddr[GIET_NB_VSPACE_MAX]; 

__attribute__((section (".kdata"))) 
page_table_t* _ptabs_vaddr[GIET_NB_VSPACE_MAX]; 

__attribute__((section (".kdata"))) 
static_scheduler_t*	_schedulers_paddr[NB_CLUSTERS*NB_PROCS_MAX]; 

//////////////////////////////////////////////////////////////////////////////////
// This function is the entry point for the last step of the boot sequence.
//////////////////////////////////////////////////////////////////////////////////
__attribute__((section (".kinit"))) void _kernel_init()
{
    // values to be written in registers
    unsigned int	sp_value;
    unsigned int	sr_value;
    unsigned int	ptpr_value;
    unsigned int	epc_value;

    unsigned int	proc_id    = _procid();
    unsigned int    cluster_id = proc_id / NB_PROCS_MAX;
    unsigned int    lpid       = proc_id % NB_PROCS_MAX;

    // step 1 : Initialise scheduler physical addresses array

    // get scheduler physical address from register
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    _schedulers_paddr[proc_id]     = psched;

#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 1 for processor ");
_putw( proc_id );
_puts("\n");
_puts("- scheduler pbase = ");
_putw( (unsigned int)psched );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif

    // step 2 : compute and set ICU mask 
    unsigned int irq_id;
    unsigned int mask = 0;
    for ( irq_id = 0 ; irq_id < 32 ; irq_id++ )
    {
        unsigned int entry   = _get_interrupt_vector_entry(irq_id);
        if ( entry ) mask = mask | 0x1<< irq_id;
    }
    _icu_write( cluster_id,
                lpid,
                ICU_MASK_SET,
                mask );
   
#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 2 for processor ");
_putw( proc_id );
_puts("\n");
_puts("- ICU mask = ");
_putw( mask );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif


    // step 3 : TODO initialise page table addresse arrays


    // step 4 : start TICK timer if more than one task
    unsigned int tasks = _get_tasks_number();
    if ( tasks > 1 )
    {
        unsigned int period     = GIET_TICK_VALUE;
        unsigned int mode       = 0x3;
        _timer_access( 0,			// write access
                       cluster_id, 
                       proc_id, 
                       TIMER_PERIOD, 
                       &period );
        _timer_access( 0,			// write access
                       cluster_id, 
                       proc_id, 
                       TIMER_MODE, 
                       &mode );
        
#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] Step 4 for processor ");
_putw( proc_id );
_puts("\n");
_puts("- TICK period = ");
_putd( period );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif
    
    } 

    // step 5 : each processor initialises SP, SR, PTPR, EPC, registers
    //          with the values corresponding to the first allocated task,
    //          It does nothing, and keep idle if no task allocated.

    if ( tasks )		// at leat one task allocated
    {
        // initialise registers
        sp_value   = _get_current_context_slot(CTX_SP_ID);
        sr_value   = _get_current_context_slot(CTX_SR_ID);
        ptpr_value = _get_current_context_slot(CTX_PTPR_ID);
        epc_value  = _get_current_context_slot(CTX_EPC_ID);

#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 5 for processor ");
_putw( proc_id );
_puts("\n");
_puts("- sp   = ");
_putw( sp_value );
_puts("\n");
_puts("- sr   = ");
_putw( sr_value );
_puts("\n");
_puts("- ptpr = ");
_putw( ptpr_value<<13 );
_puts("\n");
_puts("- epc  = ");
_putw( epc_value );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif
    }
    else					// no task allocated
    {
        _get_lock( &_tty_put_lock );
        _puts("\n No task allocated to processor ");
        _putw( proc_id );
        _puts(" => keep idle\n");
        _release_lock ( &_tty_put_lock );

        // enable interrupts in kernel mode
        asm volatile ( "li	   $26, 0xFF01  \n"
                       "mtc0   $26, $12     \n" 
                       ::: "$26" );

		// infinite loop in kernel mode
        while (1) asm volatile("nop");
    }

    // set critical registers and jump to user code
    asm volatile (
       "move	$29,	%0		\n"		/* SP <= ctx[CTX_SP_ID] */
       "mtc0	%1,		$12		\n"		/* SR <= ctx[CTX_SR_ID] */
       "mtc2	%2,		$0		\n"		/* PTPR <= ctx[CTX_PTPR_ID] */
       "mtc0	%3,		$14		\n"		/* EPC <= ctx[CTX_EPC_ID] */
       "eret					\n"		/* jump to user code */
       "nop						\n"
       :
       : "r"(sp_value), "r"(sr_value), "r"(ptpr_value), "r"(epc_value) );

} // end _kernel_init()