///////////////////////////////////////////////////////////////////////////////////
// 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 <drivers.h>
#include <stdarg.h>

////////////////////////////////////////////////////////////////////////////
// _get_lock()
////////////////////////////////////////////////////////////////////////////
inline void _get_lock( unsigned int* plock )
{
    register unsigned int  delay = (_proctime() & 0xF) << 4;

    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()
// used for system code debug / it uses TTY0 
////////////////////////////////////////////////////////////////////////////
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() 
// used for system code debug / it uses TTY0
////////////////////////////////////////////////////////////////////////////
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);
}
////////////////////////////////////////////////////////////////////////////
// _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))
                );
    }
}
///////////////////////////////////////////////////////////////////////////////////
// 	_itoa_dec()
// Convert a 32-bit unsigned integer to a string of ten decimal characters.
///////////////////////////////////////////////////////////////////////////////////
void _itoa_dec(unsigned int val, char *buf)
{
    const static char dectab[] = "0123456789";
    unsigned int i;

    for (i = 0; i < 10; i++)
    {
        if ((val != 0) || (i == 0))
            buf[9-i] = dectab[val % 10];
        else
            buf[9-i] = 0x20;
        val /= 10;
    }
}
///////////////////////////////////////////////////////////////////////////////////
// 	_itoa_hex()
// Convert a 32-bit unsigned integer to a string of eight hexadecimal characters.
///////////////////////////////////////////////////////////////////////////////////
void _itoa_hex(unsigned int val, char *buf)
{
    const static char hexatab[] = "0123456789ABCD";
    unsigned int i;

    for (i = 0; i < 8; i++)
    {
        buf[7-i] = hexatab[val % 16];
        val /= 16;
    }
}
///////////////////////////////////////////////////////////////////////////////////
// 	_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_bar()
{
    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;
}

///////////////////////////////////////////////////////////////////////////////////
//  	_it_mask()
// Access CP0 and mask IRQs
///////////////////////////////////////////////////////////////////////////////////
inline void _it_mask()
{
    asm volatile(
            "lui   $27,      0xFFFF   \n"
            "ori   $27, $27, 0xFFFE   \n"
            "mfc0  $26, $12           \n"
            "and   $26, $26, $27      \n"
            "mtc0  $26, $12           \n"
            ::: "$26", "$27"
            );
}
///////////////////////////////////////////////////////////////////////////////////
//  	_it_enable()
// Access CP0 and enable IRQs
///////////////////////////////////////////////////////////////////////////////////
inline void _it_enable()
{
    asm volatile(
            "mfc0  $26, $12      \n"
            "ori   $26, $26, 1   \n"
            "mtc0  $26, $12      \n"
            ::: "$26"
            );
}

/////////////////////////////////////////////////////////////////////////////
//  	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);
}