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

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

// SR save (used by _it_mask() / it_restore()
unsigned int _status_register_save[NB_CLUSTERS*NB_PROCS_MAX];

///////////////////////////////////////////////////////////////////////////////////
//       _get_sched()
// Access CP0 and returns a pointer (virtual address) on the calling
// processor scheduler.
///////////////////////////////////////////////////////////////////////////////////
static_scheduler_t* _get_sched() 
{
    unsigned int vaddr;
    asm volatile(
            "mfc0    %0,   $22    \n" 
            : "=r"(vaddr) );
    return (static_scheduler_t*)vaddr;
}

///////////////////////////////////////////////////////////////////////////////////
//       _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
// This function uses a global _status_register_save array
// This function is NOT USED and NOT TESTED
///////////////////////////////////////////////////////////////////////////////////
inline void _it_mask() 
{
    unsigned int sr_value;
    unsigned int proc_id;
    asm volatile(
            "li      $3,        0xFFFFFFFE    \n"
            "mfc0    %0,        $12           \n"
            "and     $3,        $3, %0        \n"
            "mtc0    $3,        $12           \n"
            "mfc0    %1,        $15, 1        \n"
            : "=r"(sr_value), "=r"(proc_id)
            : 
            : "$3");
    _status_register_save[proc_id] = sr_value;
}

///////////////////////////////////////////////////////////////////////////////////
//    _it_restore()
// Access CP0 and enable IRQs
// This function uses a global _status_register_save array
// This function is NOT USED and NOT TESTED
///////////////////////////////////////////////////////////////////////////////////
inline void _it_restore() 
{
    unsigned int proc_id;
    // get the processor id to index the _status_register_save table
    asm volatile("mfc0 %0, $15, 1" : "=r" (proc_id));
    // restore the saved value into the status register
    asm volatile("mtc0  %0, $12" : : "r" (_status_register_save[proc_id]));
}

///////////////////////////////////////////////////////////////////////////////////
//    _it_disable()
// Access CP0 and disables IRQs
///////////////////////////////////////////////////////////////////////////////////
inline void _it_disable() 
{
    asm volatile(
            "li      $3,        0xFFFFFFFE    \n"
            "mfc0    $4,        $12           \n"
            "and     $3,        $3, $4        \n"
            "mtc0    $3,        $12           \n"
            ::: "$3", "$4");
}

///////////////////////////////////////////////////////////////////////////////////
//    _it_enable()
// Access CP0 and enables IRQs
///////////////////////////////////////////////////////////////////////////////////
inline void _it_enable() 
{
    asm volatile(
            "li      $3,        0x00000001    \n"
            "mfc0    $4,        $12           \n"
            "or      $3,        $3, $4        \n"
            "mtc0    $3,        $12           \n"
            ::: "$3", "$4");
}

////////////////////////////////////////////////////////////////////////////
//    _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) 
{
    asm volatile (
            "sync\n" /* necessary because of the consistency model in tsar */
            );
    *plock = 0;
}

////////////////////////////////////////////////////////////////////////////
//    _puts()
// display a string on TTY0 / used for system code debug and 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[TTY_WRITE] = (unsigned int) buffer[n];
    }
}

////////////////////////////////////////////////////////////////////////////
//    _putx() 
// display a 32 bits unsigned int as an hexadecimal string on TTY0 
////////////////////////////////////////////////////////////////////////////
void _putx(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);
}

////////////////////////////////////////////////////////////////////////////
//    _putl() 
// display a 64 bits unsigned long as an hexadecimal string on TTY0 
////////////////////////////////////////////////////////////////////////////
void _putl(paddr_t val) 
{
    static const char HexaTab[] = "0123456789ABCDEF";
    char buf[19];
    unsigned int c;

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

    for (c = 0; c < 16; c++) 
    { 
        buf[17 - c] = HexaTab[(unsigned int)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))
                );
    }
}

/////////////////////////////////////////////////////////////////////////////
//      _get_task_slot()
// This function returns the content of a context slot 
// for the task identified by the ltid argument (local index).
/////////////////////////////////////////////////////////////////////////////
unsigned int _get_task_slot( unsigned int ltid,
                             unsigned int slot )
{
    static_scheduler_t* psched  = _get_sched();
    return psched->context[ltid][slot];
}

/////////////////////////////////////////////////////////////////////////////
//      _set_task_slot()
// This function updates the content of a context slot 
// for the task identified by the ltid argument (local index).
/////////////////////////////////////////////////////////////////////////////
void _set_task_slot( unsigned int ltid,
                     unsigned int slot,
                     unsigned int value )
{
    static_scheduler_t* psched  = _get_sched();
    psched->context[ltid][slot] = value;
}

/////////////////////////////////////////////////////////////////////////////
//      _get_context_slot()
// This function returns the content of a context slot 
// for the running task (defined by the scheduler current field).
/////////////////////////////////////////////////////////////////////////////
unsigned int _get_context_slot( unsigned int slot )
{
    static_scheduler_t* psched  = _get_sched();
    unsigned int        task_id = psched->current;
    return psched->context[task_id][slot];
}

/////////////////////////////////////////////////////////////////////////////
//      _set_context_slot()
// This function updates the content of a context slot for the running task.
/////////////////////////////////////////////////////////////////////////////
void _set_context_slot( unsigned int slot,
                       unsigned int value )
{
    static_scheduler_t* psched  = _get_sched();
    unsigned int        task_id = psched->current;
    psched->context[task_id][slot] = value;
}

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


// Local Variables:
// tab-width: 4
// c-basic-offset: 4
// c-file-offsets:((innamespace . 0)(inline-open . 0))
// indent-tabs-mode: nil
// End:
// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4