///////////////////////////////////////////////////////////////////////////////////
// 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 perform the following actions:
// 1/ contributes to _schedulers_paddr[] initialisation
// 2/ contributes to _ptabs_paddr[] and _ptabs_vaddr arrays initialisation 
// 3/ computes and set the ICU mask for its private ICU channel
// 4/ initialises its private TICK timer (if required)
// 5/ initialises the "idle" task context in its private scheduler
// 6/ initialises the SP, SR, PTPR, EPC registers
// 7/ jumps 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"))) 
unsigned int 			_ptabs_paddr[GIET_NB_VSPACE_MAX]; 

__attribute__((section (".kdata"))) 
unsigned int 			_ptabs_vaddr[GIET_NB_VSPACE_MAX]; 

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

__attribute__((section (".kdata"))) 
unsigned int			_idle_stack[NB_CLUSTERS*NB_PROCS_MAX*64]; 

void _sys_exit()
{
    while(1);
}

//////////////////////////////////////////////////////////////////////////////////
// This function is the entry point for the last step of the boot sequence.
//////////////////////////////////////////////////////////////////////////////////
__attribute__((section (".kinit"))) void _kernel_init()
{
    // compute cluster and local processor index
    unsigned int	global_pid = _procid();
    unsigned int    cluster_id = global_pid / NB_PROCS_MAX;
    unsigned int    proc_id    = global_pid % NB_PROCS_MAX;

    // Step 0 : Compute number of tasks allocated to proc

    unsigned int tasks = _get_tasks_number();

#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 0 for processor ");
_putd( global_pid );
_puts(" : tasks = ");
_putd( tasks );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif

    // step 1 : Initialise scheduler physical addresses array
    //          get scheduler physical address (from CP0 register)

    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    _schedulers_paddr[global_pid]  = psched;

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

    // step 2 : initialise page table addresse arrays
    //          each processor scans all tasks contexts in its
    //          private scheduler and get VSID, PTAB and PTPR values

    unsigned int ltid;

    for ( ltid = 0 ; ltid < tasks ; ltid++ )
    {
        unsigned int vspace_id  = _get_context_slot( ltid , CTX_VSID_ID ); 
        unsigned int ptab_vaddr = _get_context_slot( ltid , CTX_PTAB_ID ); 
        unsigned int ptab_paddr = _get_context_slot( ltid , CTX_PTPR_ID ) << 13; 

        _ptabs_vaddr[vspace_id] = ptab_vaddr;
        _ptabs_paddr[vspace_id] = ptab_paddr;

#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 2 for processor ");
_putd( global_pid );
_puts(" / vspace ");
_putd( vspace_id );
_puts("\n- ptab vbase = ");
_putx( ptab_vaddr );
_puts("\n- ptab pbase = ");
_putx( ptab_paddr );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif

    }
 
    // step 3 : compute and set ICU masks
    //          there is at most 32 interrupts per processor
    //          software interrupts are not supported yet

    unsigned int irq_id;
    unsigned int hwi_mask = 0;
    unsigned int pti_mask = 0;

    unsigned int isr_switch_channel = 0xFFFFFFFF;

    for ( irq_id = 0 ; irq_id < 32 ; irq_id++ )
    {
        unsigned int entry  = _get_interrupt_vector_entry(irq_id);
        unsigned int isr    = entry & 0x000000FF;

        if ( (isr == ISR_DMA) || (isr == ISR_IOC) || (isr == ISR_TTY) )
        {
             hwi_mask = hwi_mask | 0x1<< irq_id;
        }
        else if ( (isr == ISR_SWITCH) )
        { 
            pti_mask = pti_mask | 0x1<< irq_id;
            isr_switch_channel = irq_id;
        }
        else if ( (isr == ISR_TIMER) )
        {
            pti_mask = pti_mask | 0x1<< irq_id;
        }
    }
    _icu_set_mask( cluster_id, proc_id, hwi_mask, 0 ); // set HWI_MASK
    _icu_set_mask( cluster_id, proc_id, pti_mask, 1 );	// set PTI_MASK
   
#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 3 for processor ");
_putd( global_pid );
_puts("\n - ICU HWI_MASK = ");
_putx( hwi_mask );
_puts("\n - ICU PTI_MASK = ");
_putx( pti_mask );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif

    // step 4 : start TICK timer if more than one task
    if ( tasks > 1 )
    {
        if(isr_switch_channel == 0xFFFFFFFF)
        {
            _puts("\n[GIET ERROR] ISR_SWITCH not found on proc ");
            _putd( proc_id);
            _puts("\n");
            _sys_exit();
        }
        _timer_start( cluster_id, 
                      isr_switch_channel, 
                      GIET_TICK_VALUE );
        
#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] Step 4 for processor ");
_putd( global_pid );
_puts(" / context switch activated\n");
_release_lock(&_tty_put_lock);
#endif
    
    } 

    // step 5 : initialise the "idle" task context
    //          the SR initialisation value is 0xFF03 because
    //          the task _ctx_idle() executes in kernel mode...
    //          it uses the page table of vspace[0]
  
    _set_context_slot( IDLE_TASK_INDEX, CTX_RUN_ID,  1 );
    _set_context_slot( IDLE_TASK_INDEX, CTX_SR_ID,   0xFF03 );
    _set_context_slot( IDLE_TASK_INDEX, CTX_SP_ID,   (unsigned int)&_idle_stack[global_pid] + 64 );
    _set_context_slot( IDLE_TASK_INDEX, CTX_RA_ID,   (unsigned int)&_ctx_eret );
    _set_context_slot( IDLE_TASK_INDEX, CTX_EPC_ID,  (unsigned int)&_ctx_idle );
    _set_context_slot( IDLE_TASK_INDEX, CTX_LTID_ID, IDLE_TASK_INDEX );
    _set_context_slot( IDLE_TASK_INDEX, CTX_PTPR_ID, _ptabs_paddr[0] >> 13 );

#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] Step 5 for processor ");
_putd( global_pid );
_puts(" / idle task context set\n");
_release_lock(&_tty_put_lock);
#endif
    
    // step 6 : each processor initialises SP, SR, PTPR, EPC, registers
    //          with the values corresponding to the first allocated task,
    //          and starts the "idle" task if there is no task allocated.

    unsigned int task_id;

    if ( tasks == 0 )
    {
        task_id = IDLE_TASK_INDEX;

        _get_lock( &_tty_put_lock );
        _puts("\n [GIET WARNING] No task allocated to processor ");
        _putd( global_pid );
        _puts(" => idle\n");
        _release_lock ( &_tty_put_lock );
    }
    else
    {
       	task_id = 0;
    }
        
    unsigned int    sp_value   = _get_context_slot( task_id, CTX_SP_ID );
    unsigned int    sr_value   = _get_context_slot( task_id, CTX_SR_ID );
    unsigned int    ptpr_value = _get_context_slot( task_id, CTX_PTPR_ID );
    unsigned int    epc_value  = _get_context_slot( task_id, CTX_EPC_ID );

#if GIET_DEBUG_INIT
_get_lock(&_tty_put_lock);
_puts("\n[GIET DEBUG] step 6 for processor ");
_putd( global_pid );
_puts(" / registers initialised \n");
_puts("- sp   = ");
_putx( sp_value );
_puts("\n");
_puts("- sr   = ");
_putx( sr_value );
_puts("\n");
_puts("- ptpr = ");
_putx( ptpr_value<<13 );
_puts("\n");
_puts("- epc  = ");
_putx( epc_value );
_puts("\n");
_release_lock(&_tty_put_lock);
#endif

    // set  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()