///////////////////////////////////////////////////////////////////////////////////
// File     : common.c
// Date     : 01/04/2012
// Author   : alain greiner and joel porquet
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////
// The common.c and common.h files are part of the GIET nano-kernel.
// They contains various utilities functions.
///////////////////////////////////////////////////////////////////////////////////

#include <sys_handler.h>
#include <common.h>
#include <ctx_handler.h>
#include <drivers.h>
#include <stdarg.h>

///////////////////////////////////////////////////////////////////////////////////
//	Global variables
///////////////////////////////////////////////////////////////////////////////////

// current context cache TODO

// SR save (used by _it_mask() / it_restore()
unsigned int	_status_register_save;

///////////////////////////////////////////////////////////////////////////////////
// 	  _get_sched()
// Access CP0 and returns scheduler physical address.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_sched()
{
    unsigned int ret;
    asm volatile (	"mfc0	%0,		$22" 
					: "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// 	  _get_ptpr()
// Access CP2 and returns PTPR register.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_ptpr()
{
    unsigned int ret;
    asm volatile (	"mfc2	%0,		$0" 
					: "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// 	  _get_epc()
// Access CP0 and returns EPC register.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_epc()
{
    unsigned int ret;
    asm volatile (	"mfc0	%0,		$14" 
					: "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// 	  _get_bar()
// Access CP0 and returns BAR register.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_bvar()
{
    unsigned int ret;
    asm volatile (	"mfc0	%0,		$8" 
					: "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// 	  _get_cr()
// Access CP0 and returns CR register.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_cause()
{
    unsigned int ret;
    asm volatile (	"mfc0	%0,		$13" 
					: "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// 	  _get_sr()
// Access CP0 and returns SR register.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_sr()
{
    unsigned int ret;
    asm volatile (	"mfc0	%0,		$12" 
					: "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
//    _it_mask()
// Access CP0 and mask IRQs
///////////////////////////////////////////////////////////////////////////////////
inline void _it_mask()
{
    unsigned int	sr_value;
    asm volatile(	"li		$3,		0xFFFFFFFE	\n"
					"mfc0	%0,		$12			\n"
					"and	$3,		$3, %0		\n"
					"mtc0	$3,		$12			\n"
					: "=r"(sr_value) : : "$3" );
    _status_register_save = sr_value;
}
///////////////////////////////////////////////////////////////////////////////////
//    _it_enable()
// Access CP0 and enable IRQs
///////////////////////////////////////////////////////////////////////////////////
inline void _it_restore()
{
    unsigned int	sr_value = _status_register_save;
    asm volatile(	"mtc0  %0,		$12			\n"
					: : "r"(sr_value) );
}
////////////////////////////////////////////////////////////////////////////
//    _get_lock()
// Takes a lock with an ll/sc atomic access.
// A pseudo random delay is introduced before retry in case of miss
// (delay average value = 100 cycles)
////////////////////////////////////////////////////////////////////////////
inline void _get_lock( unsigned int* plock )
{
    register unsigned int  delay = ( _proctime() ^ _procid()<<4 ) & 0xFF;

    asm volatile (
            "_lock_llsc:             \n"
            "ll   $2,    0(%0)       \n" /* $2 <= _ioc_lock current value */
            "bnez $2,    _lock_delay \n" /* delay if _ioc_lock already taken */
            "li   $3,    1           \n" /* $3 <= argument for sc */
            "sc   $3,    0(%0)       \n" /* try to set _ioc_lock */
            "bnez $3,    _lock_ok    \n" /* exit if atomic */
            "_lock_delay:            \n"
            "move $4,    %1          \n" /* $4 <= delay */
            "_lock_loop:             \n"
            "addi $4,    $4,    -1   \n" /* $4 <= $4 - 1 */
            "beqz $4,    _lock_loop  \n" /* test end delay */
            "j           _lock_llsc  \n" /* retry */
            "_lock_ok:               \n"
            :
            :"r"(plock), "r"(delay)
            :"$2", "$3", "$4");
}

////////////////////////////////////////////////////////////////////////////
// _release_lock()
////////////////////////////////////////////////////////////////////////////
inline void _release_lock( unsigned int* plock )
{
    *plock = 0;
}

////////////////////////////////////////////////////////////////////////////
//    _puts()
// display a string on TTY0 / used for system code debugand log
////////////////////////////////////////////////////////////////////////////
void _puts(char *buffer) 
{
    unsigned int* tty_address = (unsigned int*)&seg_tty_base;
    unsigned int n;

    for ( n=0; n<100; n++)
    {
        if (buffer[n] == 0) break;
        tty_address[0] = (unsigned int)buffer[n];
    }
}
////////////////////////////////////////////////////////////////////////////
//    _putw() 
// display an int (hexa) on TTY0 / used for system code debug and log
////////////////////////////////////////////////////////////////////////////
void _putw(unsigned int val)
{
    static const char   HexaTab[] = "0123456789ABCDEF";
    char                buf[11];
    unsigned int        c;

    buf[0]  = '0';
    buf[1]  = 'x';
    buf[10] = 0;

    for ( c = 0 ; c < 8 ; c++ )
    { 
        buf[9-c] = HexaTab[val&0xF];
        val = val >> 4;
    }
    _puts(buf);
}
////////////////////////////////////////////////////////////////////////////
//    _putd() 
// display an int (decimal) on TTY0 / used for system code debug and log
////////////////////////////////////////////////////////////////////////////
void _putd(unsigned int val)
{
    static const char   DecTab[] = "0123456789";
    char				buf[11];
    unsigned int		i;
    unsigned int		first;

    buf[10] = 0;

    for (i = 0; i < 10; i++)
    {
        if ((val != 0) || (i == 0))
        {
            buf[9-i] = DecTab[val % 10];
            first    = 9-i;
        }
        else
        {
            break;
        }
        val /= 10;
    }
    _puts( &buf[first] );
}
////////////////////////////////////////////////////////////////////////////
//    _strncmp()
// compare two strings s1 & s2 (no more than n characters)
////////////////////////////////////////////////////////////////////////////
unsigned int _strncmp(const char* s1, 
                      const char* s2, 
                      unsigned int n)
{
    unsigned int i;
    for ( i=0 ; i<n ; i++)
    {
        if ( s1[i] != s2[i] ) return 1;
	if ( s1[i] == 0 )     break;
    }
    return 0;
}
////////////////////////////////////////////////////////////////////////////
// 	   _dcache_buf_invalidate()
// Invalidate all data cache lines corresponding to a memory 
// buffer (identified by an address and a size).
////////////////////////////////////////////////////////////////////////////
void _dcache_buf_invalidate(const void *buffer, 
                            unsigned int size)
{
    unsigned int i;
    unsigned int tmp;
    unsigned int line_size;

    // compute data cache line size based on config register (bits 12:10)
    asm volatile("mfc0 %0, $16, 1" : "=r"(tmp));
    tmp = ((tmp>>10) & 0x7);
    line_size = 2 << tmp;

    // iterate on cache lines 
    for (i = 0; i < size; i += line_size)
    {
        asm volatile(
                " cache %0, %1"
                ::"i" (0x11), "R" (*((unsigned char*)buffer+i))
                );
    }
}
////////////////////////////////////////////////////////////////////////////
//    _physical_read_access()
// This function makes a physical read access to a 32 bits word in memory, 
// after a temporary DTLB desactivation. 
////////////////////////////////////////////////////////////////////////////
unsigned int _physical_read_access(unsigned int* paddr)
{
    unsigned int value;

    asm volatile(   "li     $3,     0xFFFFFFFE		\n"
                    "mfc0   $2,     $12				\n"		/* $2 <= SR        */
                    "and    $3,     $3,		$2		\n"
                    "mtc0   $3,     $12				\n"		/* interrupt masked */
                    "li		$3,		0xB				\n"
                    "mtc2	$3,		$1				\n"		/* DTLB off			*/	

                    "lw		%0,		0(%1)			\n"		/* entry <= *pslot	*/

                    "li		$3,		0xF				\n"
                    "mtc2	$3,		$1				\n"		/* DTLB on 			*/	
                    "mtc0	$2,		$12				\n"		/* restore SR		*/
                    : "=r"(value)
                    : "r"(paddr)
                    : "$2", "$3" );
    return value;
}
////////////////////////////////////////////////////////////////////////////
//    _physical_write_access()
// This function makes a physical write access to a 32 bits word in memory, 
// after a temporary DTLB desactivation. 
////////////////////////////////////////////////////////////////////////////
void _physical_write_access(unsigned int* paddr, unsigned int value)
{
    asm volatile(   "li     $3,     0xFFFFFFFE		\n"
                    "mfc0   $2,     $12				\n"		/* $26 <= SR        */
                    "and    $3,     $3,		$2		\n"
                    "mtc0   $3,     $12				\n"		/* interrupt masked */
                    "li		$3,		0xB				\n"
                    "mtc2	$3,		$1				\n"		/* DTLB off			*/
	
                    "sw		%0,		0(%1)			\n"		/* entry <= *pslot	*/

                    "li		$3,		0xF				\n"
                    "mtc2	$3,		$1				\n"		/* DTLB on 			*/	
                    "mtc0	$2,		$12				\n"		/* restore SR		*/
                    :
                    : "r"(value), "r"(paddr)
                    : "$2", "$3" );
}
////////////////////////////////////////////////////////////////////////////
//    _get_tasks_number()
// This function returns the number of tasks allocated to processor.
////////////////////////////////////////////////////////////////////////////
unsigned int _get_tasks_number()
{
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    return _physical_read_access( &(psched->tasks) );
}
////////////////////////////////////////////////////////////////////////////
//    _get_current_task_id()
// This function returns the index of the currently running task.
////////////////////////////////////////////////////////////////////////////
unsigned int _get_current_task_id()
{
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    return _physical_read_access( &(psched->current) );
}
////////////////////////////////////////////////////////////////////////////
//    _set_current_task_id()
// This function returns the index of the currently running task.
////////////////////////////////////////////////////////////////////////////
void _set_current_task_id( unsigned int value )
{
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    _physical_write_access( &(psched->current), value );
}
///////////////////////////////////////////////////////////////////////////////
//    _get_context_slot()
// This function returns a slot content for the task defined by task_id.
///////////////////////////////////////////////////////////////////////////////
unsigned int _get_context_slot( unsigned int task_id,
                                unsigned int slot_id )
{
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    return _physical_read_access( &(psched->context[task_id][slot_id]) );
}
///////////////////////////////////////////////////////////////////////////////
//    _set_context_slot()
// This function returns a slot content for the task defined by task_id.
///////////////////////////////////////////////////////////////////////////////
void _set_context_slot( unsigned int task_id,
                        unsigned int slot_id,
                        unsigned int value )
{
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    _physical_write_access( &(psched->context[task_id][slot_id]), value );
}
////////////////////////////////////////////////////////////////////////////////
//    _get_interrupt_vector_entry()
// This function returns the interrupt_vector entry defined by argument index.
////////////////////////////////////////////////////////////////////////////////
unsigned int _get_interrupt_vector_entry( unsigned int index )
{
    static_scheduler_t*		psched = (static_scheduler_t*)_get_sched();
    return _physical_read_access( &(psched->interrupt_vector[index]) );
}

/////////////////////////////////////////////////////////////////////////////
//  	access functions to mapping_info data structure
/////////////////////////////////////////////////////////////////////////////
mapping_cluster_t* _get_cluster_base( mapping_header_t* header )
{
    return   (mapping_cluster_t*) ((char*)header +
                                  MAPPING_HEADER_SIZE);
}
/////////////////////////////////////////////////////////////////////////////
mapping_pseg_t* _get_pseg_base( mapping_header_t* header )
{
    return   (mapping_pseg_t*)    ((char*)header +
                                  MAPPING_HEADER_SIZE +
                                  MAPPING_CLUSTER_SIZE*header->clusters);
}
/////////////////////////////////////////////////////////////////////////////
mapping_vspace_t* _get_vspace_base( mapping_header_t* header )
{
    return   (mapping_vspace_t*)  ((char*)header +
                                  MAPPING_HEADER_SIZE +
                                  MAPPING_CLUSTER_SIZE*header->clusters +
                                  MAPPING_PSEG_SIZE*header->psegs);
}
/////////////////////////////////////////////////////////////////////////////
mapping_vseg_t* _get_vseg_base( mapping_header_t* header )
{
    return   (mapping_vseg_t*)    ((char*)header +
                                  MAPPING_HEADER_SIZE +
                                  MAPPING_CLUSTER_SIZE*header->clusters +
                                  MAPPING_PSEG_SIZE*header->psegs +
                                  MAPPING_VSPACE_SIZE*header->vspaces);
}
/////////////////////////////////////////////////////////////////////////////
mapping_vobj_t* _get_vobj_base( mapping_header_t* header )
{
    return   (mapping_vobj_t*)   ((char*)header +
                                  MAPPING_HEADER_SIZE +
                                  MAPPING_CLUSTER_SIZE*header->clusters +
                                  MAPPING_PSEG_SIZE*header->psegs +
                                  MAPPING_VSPACE_SIZE*header->vspaces +
                                  MAPPING_VSEG_SIZE*header->vsegs );
}
/////////////////////////////////////////////////////////////////////////////
mapping_task_t* _get_task_base( mapping_header_t* header )
{
    return   (mapping_task_t*)    ((char*)header +
                                  MAPPING_HEADER_SIZE +
                                  MAPPING_CLUSTER_SIZE*header->clusters +
                                  MAPPING_PSEG_SIZE*header->psegs +
                                  MAPPING_VSPACE_SIZE*header->vspaces +
                                  MAPPING_VOBJ_SIZE*header->vobjs +
                                  MAPPING_VSEG_SIZE*header->vsegs);
}