///////////////////////////////////////////////////////////////////////////////////
// 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
///////////////////////////////////////////////////////////////////////////////////

// current context cache TODO

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

///////////////////////////////////////////////////////////////////////////////////
//       _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
// 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 an int (hexa) on TTY0 / used for system code debug and log
////////////////////////////////////////////////////////////////////////////
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);
}


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


// 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