///////////////////////////////////////////////////////////////////////////////////
// File     : utils.c
// Date     : 18/10/2013
// Author   : alain greiner
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////
// The utils.c and utils.h files are part of the GIET-VM nano-kernel.
// They define more or less the GIET-VM HAL (Hardware Abstraction Layer),
// and contains various utility functions, that can be used by both the 
// boot code and the kernel code.
///////////////////////////////////////////////////////////////////////////////////

#include <giet_config.h>
#include <hard_config.h>
#include <mapping_info.h>
#include <utils.h>
#include <ctx_handler.h>
#include <tty_driver.h>
#include <stdarg.h>

// This global variable is allocated in the boot.c file or in kernel_init.c file
extern static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)];

///////////////////////////////////////////////////////////////////////////////////
// This function implements a pseudo-random delay.
// The val argument define approximately an exponentially increasing mean delay,
// and should not be larger than 32.
///////////////////////////////////////////////////////////////////////////////////
inline void _random_wait( unsigned int val )
{
    unsigned int mask  = (1<<(val&0x1F))-1;
    unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
    asm volatile( "move  $3,   %0                 \n"
                  "loop_nic_completed:            \n"
                  "addi  $3,   $3, -1             \n"
                  "bnez  $3,   loop_nic_completed \n"
                  "nop                            \n"
                  :
                  : "r" (delay)
                  : "$3" ); 
}
///////////////////////////////////////////////////////////////////////////////////
// Copy a source memory buffer content to a dest memory buffer (size bytes)
// Code taken from MutekH.
///////////////////////////////////////////////////////////////////////////////////
inline void* memcpy( void*        dest,     // dest buffer vbase
                     const void*  source,   // source buffer vbase
                     unsigned int size )    // bytes
{
    unsigned int* idst = (unsigned int*)dest;
    unsigned int* isrc = (unsigned int*)source;

    // word-by-word copy
    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
    {
        while (size > 3) 
        {
            *idst++ = *isrc++;
            size -= 4;
        }
    }

    unsigned char* cdst = (unsigned char*)dest;
    unsigned char* csrc = (unsigned char*)source;

    /* byte-by-byte copy */
    while (size--) 
    {
        *cdst++ = *csrc++;
    }
    return dest;
}
//////////////////////////////////////////////////////////////////////////////////
// Fill a byte string with a byte value.
//////////////////////////////////////////////////////////////////////////////////
inline void * memset( void*        dest, 
                      int          value, 
                      unsigned int count ) 
{
    // word-by-word copy
    unsigned int* idst = dest;
    unsigned int  data = (((unsigned char)value)      ) |
                         (((unsigned char)value) <<  8) |
                         (((unsigned char)value) << 16) |
                         (((unsigned char)value) << 24) ;

    if ( ! ((unsigned int)idst & 3) )
    {
        while ( count > 3 )
        {
            *idst++ = data;
            count -= 4;
        }
    }
   
    // byte-by-byte copy
    unsigned char* cdst = dest;
    while (count--) 
    {
        *cdst++ = (unsigned char)value;
    }
    return dest;
}

//////////////////////////////////////////////////////////////////////////////////
// This function implements an interactive break for debug.
// Execution continue when typing any character on TTY0. 
// The "str" argument is supposed to indicate the break location.
//////////////////////////////////////////////////////////////////////////////////
inline void _break( char* string ) 
{
    char byte;

    _printf("\n[GIET DEBUG] break from %s / continue ?\n", string );
    _getc( &byte );
}

//////////////////////////////////////////////////////////////////////////////////
// Processor suicide: infinite loop  
//////////////////////////////////////////////////////////////////////////////////
__attribute__((noreturn))
inline void _exit() 
{
    unsigned int procid     = _get_procid();
    unsigned int lpid       = procid % NB_PROCS_MAX;
    unsigned int cluster_xy = procid / NB_PROCS_MAX;
    unsigned int x          = cluster_xy >> Y_WIDTH;
    unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);


    _printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid );

    while (1) { asm volatile ("nop"); }
}
///////////////////////////////////////////////////////////////////////////////////
//         CP0 and CP2 registers access functions
///////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////
// Returns the value contained in CP0 SCHED register
// (virtual base address of the processor scheduler).
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_sched() 
{
    unsigned int ret;
    asm volatile( "mfc0      %0,     $4,2    \n" 
                  : "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// Returns PTPR register content.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_mmu_ptpr() 
{
    unsigned int ret;
    asm volatile( "mfc2      %0,     $0      \n"
                  : "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// Returns MODE register content.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_mmu_mode() 
{
    unsigned int ret;
    asm volatile( "mfc2      %0,     $1      \n"
                  : "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// Returns EPC register content.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_epc() 
{
    unsigned int ret;
    asm volatile( "mfc0      %0,    $14     \n"
                  : "=r"(ret) );
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// Returns BVAR register content.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_bvar() 
{
    unsigned int ret;
    asm volatile( "mfc0      %0,    $8     \n"
                  : "=r"(ret));
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// Returns CR register content.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_cr() 
{
    unsigned int ret;
    asm volatile( "mfc0      %0,    $13    \n"
                  : "=r"(ret));
    return ret;
}
///////////////////////////////////////////////////////////////////////////////////
// Returns SR register content
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_sr() 
{
    unsigned int ret;
    asm volatile( "mfc0      %0,     $12   \n"
                  : "=r"(ret));
    return ret;
}
//////////////////////////////////////////////////////////////////////////////
// This function set a new value for the CP0 status register.
//////////////////////////////////////////////////////////////////////////////
inline void _set_sr(unsigned int val) 
{
    asm volatile( "mtc0      %0,     $12    \n"
                  :
                  :"r" (val) );
}
//////////////////////////////////////////////////////////////////////////////
// Returns processor index
//////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_procid() 
{
    unsigned int ret;
    asm volatile ( "mfc0     %0,     $15, 1  \n"
                   :"=r" (ret) );
    return (ret & 0x3FF);
}
//////////////////////////////////////////////////////////////////////////////
// Returns local time (32 bits value)
// boot_proctime() 
//////////////////////////////////////////////////////////////////////////////
inline unsigned int _get_proctime() 
{
    unsigned int ret;
    asm volatile ( "mfc0     %0,     $9      \n"
                   :"=r" (ret) );
    return ret;
}
//////////////////////////////////////////////////////////////////////////////
// Returns index of the currently running task from the processor scheduler.
//////////////////////////////////////////////////////////////////////////////
unsigned int _get_current_task_id() 
{
    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
    return (unsigned int) (psched->current);
}

//////////////////////////////////////////////////////////////////////////////
// Save SR value into save_sr_ptr variable and disable IRQs. 
//////////////////////////////////////////////////////////////////////////////
inline void _it_disable( unsigned int * save_sr_ptr) 
{
    unsigned int sr = 0;
    asm volatile( "li      $3,        0xFFFFFFFE    \n"
                  "mfc0    %0,        $12           \n"
                  "and     $3,        $3,   %0      \n"  
                  "mtc0    $3,        $12           \n" 
                  : "+r"(sr)
                  :
                  : "$3" );
    *save_sr_ptr = sr;
}
//////////////////////////////////////////////////////////////////////////////
// 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", "memory" );
}

//////////////////////////////////////////////////////////////////////////////
// Restores previous SR value.
//////////////////////////////////////////////////////////////////////////////
inline void _it_restore( unsigned int * save_sr_ptr ) 
{
    unsigned int sr = *save_sr_ptr;
    asm volatile( "mtc0    %0,        $12           \n" 
                  :
                  : "r"(sr)
                  : "memory" );
}

//////////////////////////////////////////////////////////////////////////////
// This function set a new value for the MMU PTPR register.
//////////////////////////////////////////////////////////////////////////////
inline void _set_mmu_ptpr(unsigned int val) 
{
    asm volatile ( "mtc2     %0,     $0            \n"
                   :
                   :"r" (val)
                   :"memory" );
}
//////////////////////////////////////////////////////////////////////////////
// This function set a new value for the MMU MODE register.
//////////////////////////////////////////////////////////////////////////////
inline void _set_mmu_mode(unsigned int val) 
{
    asm volatile ( "mtc2     %0,     $1             \n"
                   :
                   :"r" (val)
                   :"memory" );
}
//////////////////////////////////////////////////////////////////////////////
// This function set a new value in CP0 SCHED register.
// (virtual base address of the processor scheduler).
//////////////////////////////////////////////////////////////////////////////
inline void _set_sched(unsigned int val) 
{
    asm volatile ( "mtc0     %0,     $4, 2          \n"
                   :
                   :"r" (val) );
}

////////////////////////////////////////////////////////////////////////////
//          Physical addressing related functions
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// This function makes a physical read access to a 32 bits word in memory, 
// after a temporary DTLB de-activation and paddr extension.
////////////////////////////////////////////////////////////////////////////
inline unsigned int _physical_read( unsigned long long paddr ) 
{
    unsigned int value;
    unsigned int lsb = (unsigned int) paddr;
    unsigned int msb = (unsigned int) (paddr >> 32);
    unsigned int sr;

    _it_disable(&sr);

    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
                  "andi   $3,     $2,        0xb     \n"
                  "mtc2   $3,     $1                 \n"     /* DTLB off         */    

                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
                  "lw     %0,     0(%1)              \n"     /* value <= *paddr  */
                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   

                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
                  : "=r" (value)
                  : "r" (lsb), "r" (msb)
                  : "$2", "$3" );

    _it_restore(&sr);
    return value;
}
////////////////////////////////////////////////////////////////////////////
// This function makes a physical write access to a 32 bits word in memory, 
// after a temporary DTLB de-activation and paddr extension.
////////////////////////////////////////////////////////////////////////////
inline void _physical_write( unsigned long long paddr, 
                             unsigned int       value ) 
{
    unsigned int lsb = (unsigned int)paddr;
    unsigned int msb = (unsigned int)(paddr >> 32);
    unsigned int sr;

    _it_disable(&sr);

    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
                  "andi   $3,     $2,        0xb     \n"
                  "mtc2   $3,     $1                 \n"     /* DTLB off         */    

                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
                  "sw     %0,     0(%1)              \n"     /* *paddr <= value  */
                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   

                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
                  "sync                              \n"
                  :
                  : "r" (value), "r" (lsb), "r" (msb)
                  : "$2", "$3" );

    _it_restore(&sr);
}

///////////////////////////////////////////////////////////////////////////////////
// This function makes a memcpy from a source buffer to a destination buffer
// using physical addresses, after a temporary DTLB de-activation.
// source and destination buffers must be aligned, and size must be
// multiple of 4 bytes. 
///////////////////////////////////////////////////////////////////////////////////
inline void _physical_memcpy( unsigned long long dst_paddr,  // dest buffer paddr
                              unsigned long long src_paddr,  // source buffer paddr
                              unsigned int size )            // bytes
{
    // check alignment constraints
    if ( (dst_paddr & 3) || (src_paddr & 3) || (size & 3) ) 
    {
        _printf("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n");
        _exit();
    }

    unsigned int src_lsb = (unsigned int)src_paddr;
    unsigned int src_msb = (unsigned int)(src_paddr >> 32);
    unsigned int dst_lsb = (unsigned int)dst_paddr;
    unsigned int dst_msb = (unsigned int)(dst_paddr >> 32);
    unsigned int iter    = size>>2;
    unsigned int data;
    unsigned int sr;

    _it_disable(&sr);

    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= current MMU_MODE */
                  "andi   $3,     $2,        0xb     \n"     /* $3 <= new MMU_MODE     */
                  "mtc2   $3,     $1                 \n"     /* DTLB off               */    

                  "move   $4,     %5                 \n"     /* $4 < iter              */
                  "move   $5,     %1                 \n"     /* $5 < src_lsb           */
                  "move   $6,     %3                 \n"     /* $6 < src_lsb           */

                  "ph_memcpy_loop:                   \n"
                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= src_msb   */   
                  "lw     %0,     0($5)              \n"     /* data <= *src_paddr     */
                  "mtc2   %4,     $24                \n"     /* PADDR_EXT <= dst_msb   */   
                  "sw     %0,     0($6)              \n"     /* *dst_paddr <= data     */

                  "addi   $4,     $4,       -1       \n"     /* iter = iter - 1        */
                  "addi   $5,     $5,        4       \n"     /* src_lsb += 4           */
                  "addi   $6,     $6,        4       \n"     /* dst_lsb += 4           */
                  "bne    $4,     $0, ph_memcpy_loop \n"
                  "nop                               \n"

                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0         */   
                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE       */
                  : "=r" (data)
                  : "r" (src_lsb), "r" (src_msb), "r" (dst_lsb), "r"(dst_msb), "r"(iter)
                  : "$2", "$3", "$4", "$5", "$6" );

    _it_restore(&sr);
}

///////////////////////////////////////////////////////////////////////////////////
// This function is used by several drivers (_xxx_set_register() function)
// If the MMU is not activated, the virtual address is extended using
// X_IO and Y_IO to reach the cluster_io.
///////////////////////////////////////////////////////////////////////////////////
inline void _io_extended_write( unsigned int*  vaddr,
                                unsigned int   value )
{
    unsigned long long paddr;

    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
    {
        *vaddr = value;
    }
    else                          // use paddr extension for IO
    {
        paddr = (unsigned long long)(unsigned int)vaddr +
                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
        _physical_write( paddr, value );
    }
    asm volatile("sync" ::: "memory");
}

///////////////////////////////////////////////////////////////////////////////////
// This function is used by all drivers (_xxx_get_register() function)
// If the MMU is not activated, the virtual address is extended using
// X_IO and Y_IO to reach the cluster_io.
///////////////////////////////////////////////////////////////////////////////////
inline unsigned int _io_extended_read( unsigned int*  vaddr )
{
    unsigned long long paddr;

    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
    {
        return *(volatile unsigned int*)vaddr;
    }
    else                          // use paddr extension for IO
    {
        paddr = (unsigned long long)(unsigned int)vaddr +
                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
        return _physical_read( paddr );
    }
}

///////////////////////////////////////////////////////////////////////////////////
//     Locks access functions
///////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////
// Takes a lock with a blocking ll/sc atomic access.
// When the cache coherence is granted by the hardware,
// the first read is a standard (cacheable) lw, as the local copy
// can be polled when the lock is already taken by another task, reducing 
// trafic on the interconnect. When the lock is released by the owner task,
// the local copy is updated or invalidated by the coherence protocol.
// If there is no hardware cache coherence a random delay is introduced
// betwween two successive retry.
///////////////////////////////////////////////////////////////////////////////////
inline void _get_lock(unsigned int * plock) 
{
#if NO_HARD_CC

    register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;

    if (delay == 0) delay++;

    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 */
            "bnez $4,    _lock_loop  \n" /* test end delay */
            "nop                     \n"
            "j           _lock_llsc  \n" /* retry */
            "nop                     \n"
            "_lock_ok:               \n"
            :
            :"r"(plock), "r"(delay)
            :"$2", "$3", "$4", "memory");
#else

    asm volatile (
            "_lock_llsc:                 \n"
            "    lw   $2,    0(%0)       \n" /* dcache <= _ioc_lock current value    */
            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken          */
            "    nop                     \n"
            "    ll   $2,    0(%0)       \n" /* ll_buffer <= _ioc_lock current value */
            "    bnez $2,    _lock_llsc  \n" /* retry if _ioc_lock already taken     */
            "    li   $3,    1           \n" /* $3 <= argument for sc                */
            "    sc   $3,    0(%0)       \n" /* try to set _ioc_lock                 */
            "    beqz $3,    _lock_llsc  \n" /* retry if sc failure                  */
            "    nop                     \n"
            :
            :"r"(plock)
            :"$2", "$3", "memory");
#endif

//  register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;

//  if (delay == 0) delay++;

//  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 */
//          "bnez $4,    _lock_loop  \n" /* test end delay */
//          "nop                     \n"
//          "j           _lock_llsc  \n" /* retry */
//          "nop                     \n"ache
//          "_lock_ok:               \n"
//          :
//          :"r"(plock), "r"(delay)
//          :"$2", "$3", "$4");

}

///////////////////////////////////////////////////////////////////////////////////
// Release a previouly taken lock.
///////////////////////////////////////////////////////////////////////////////////
inline void _release_lock(unsigned int * plock) 
{
    asm volatile ( "sync\n" ::: "memory" ); 
    // sync is necessary because of the TSAR consistency model 
    *plock = 0;
}

///////////////////////////////////////////////////////////////////////////////////
//           Access functions to system terminal TTY0
///////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////
// Display "string" argument on TTY0.
// It uses the low level access functions from TTY driver, using a busy waiting
// policy if TTY buffer is full.
// The exclusive access lock should be taken by the caller.
///////////////////////////////////////////////////////////////////////////////////
void _puts( char* string ) 
{
    unsigned int n = 0;

    while ( string[n] > 0 )
    {
        // test status register
        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );

        // write one byte
        if ( string[n] == '\n') {
            _tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' );
        }
        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
        n++;
    }
}

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

///////////////////////////////////////////////////////////////////////////////////
// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
///////////////////////////////////////////////////////////////////////////////////
void _putl( unsigned long long 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 );
}

///////////////////////////////////////////////////////////////////////////////////
// Display a 32 bits unsigned int as a decimal string on TTY0.
///////////////////////////////////////////////////////////////////////////////////
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] );
}

///////////////////////////////////////////////////////////////////////////////////
// Display a format on TTY0.
// To provide an atomic display, this function takes the lock protecting
// exclusive access to TTY0, entering a critical section until the lock
// is released.
// Only a limited number of formats are supported:
//   - %d : 32 bits signed   decimal
//   - %u : 32 bits unsigned decimal
//   - %x : 32 bits unsigned hexa
//   - %l : 64 bits unsigned hexa
//   - %c : char
//   - %s : string
///////////////////////////////////////////////////////////////////////////////////
void _printf( char * format, ... ) 
{
    va_list ap;
    va_start(ap, format);
    unsigned int save_sr;     // to save SR value in critical section

    // get TTY0 lock
    _tty_get_lock( 0, &save_sr );

printf_text:

    while (*format) 
    {
        unsigned int i;
        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
        if (i) 
        {
            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
            format += i;
        }
        if (*format == '%') 
        {
            format++;
            goto printf_arguments;
        }
    }

    // release TTY0 lock
    _tty_release_lock( 0, &save_sr );

    va_end(ap);
    return;

printf_arguments:

    {
        char buf[20];
        char * pbuf;
        unsigned int len = 0;
        static const char HexaTab[] = "0123456789ABCDEF";
        unsigned int i;

        switch (*format++) 
        {
            case ('c'):             /* char conversion */
            {
                int val = va_arg( ap, int );
                len = 1;
                buf[0] = val;
                pbuf = &buf[0];
                break;
            }
            case ('d'):             /* 32 bits decimal signed  */
            {
                int val = va_arg( ap, int );
                if (val < 0) 
                {
                    val = -val;
                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
                }
                for(i = 0; i < 10; i++) 
                {
                    buf[9 - i] = HexaTab[val % 10];
                    if (!(val /= 10)) break;
                }
                len =  i + 1;
                pbuf = &buf[9 - i];
                break;
            }
            case ('u'):             /* 32 bits decimal unsigned  */
            {
                unsigned int val = va_arg( ap, unsigned int );
                for(i = 0; i < 10; i++) 
                {
                    buf[9 - i] = HexaTab[val % 10];
                    if (!(val /= 10)) break;
                }
                len =  i + 1;
                pbuf = &buf[9 - i];
                break;
            }
            case ('x'):             /* 32 bits hexadecimal unsigned */
            {
                unsigned int val = va_arg( ap, unsigned int );
                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
                for(i = 0; i < 8; i++) 
                {
                    buf[7 - i] = HexaTab[val % 16];
                    if (!(val /= 16))  break;
                }
                len =  i + 1;
                pbuf = &buf[7 - i];
                break;
            }
            case ('l'):            /* 64 bits hexadecimal unsigned */
            {
                unsigned long long val = va_arg( ap, unsigned long long );
                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
                for(i = 0; i < 16; i++) 
                {
                    buf[15 - i] = HexaTab[val % 16];
                    if (!(val /= 16))  break;
                }
                len =  i + 1;
                pbuf = &buf[15 - i];
                break;
            }
            case ('s'):             /* string */
            {
                char* str = va_arg( ap, char* );
                while (str[len]) 
                {
                    len++;
                }
                pbuf = str;
                break;
            }
            default:
                goto return_error;
        }

        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
        
        goto printf_text;
    }

return_error:

    {
        unsigned int procid     = _get_procid();
        unsigned int lpid       = procid % NB_PROCS_MAX;
        unsigned int cluster_xy = procid / NB_PROCS_MAX;
        unsigned int x          = cluster_xy >> Y_WIDTH;
        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);

        _puts("\n\n[GIET ERROR] in _printf() for processor[");
        _putd( x );
        _puts(",");
        _putd( y );
        _puts(",");
        _putd( lpid );
        _puts("]\n");

        // release TTY0 lock
        _tty_release_lock( 0, &save_sr );

        _exit();
    }
}

///////////////////////////////////////////////////////////////////////////////////
// Get a character from TTY0.
///////////////////////////////////////////////////////////////////////////////////
void _getc( char*        byte )
{
    // test status register
    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );

    // read one byte
    *byte = (char)_tty_get_register( 0, TTY_READ );
}

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

///////////////////////////////////////////////////////////////////////////////////
// Copy source string to dest string
///////////////////////////////////////////////////////////////////////////////////
char* _strcpy( char* dest, char* source )
{
    if (!dest || !source) return dest;

    while (*source)
        *(dest++) = *(source++);

    return dest;
}

///////////////////////////////////////////////////////////////////////////////////
// Invalidate all data cache lines corresponding to a memory 
// buffer (identified by an address and a size).
// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
// register, to be more processor independant.
///////////////////////////////////////////////////////////////////////////////////
void _dcache_buf_invalidate( 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)) );
    }
}

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

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

////////////////////////////////////////////////////////////////////////////////////
// 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  = (static_scheduler_t*)_get_sched();
    unsigned int        task_id = psched->current;
    return psched->context[task_id][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  = (static_scheduler_t*)_get_sched();
    unsigned int        task_id = psched->current;
    psched->context[task_id][slot] = value;
}

///////////////////////////////////////////////////////////////////////////////////
// This function returns the information associated to a heap : size and vaddr.
// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
// vspace index (CTX_VSID_ID) defined in the task context. 
///////////////////////////////////////////////////////////////////////////////////
unsigned int _heap_info( unsigned int* vaddr, 
                         unsigned int* size ) 
{
    mapping_header_t * header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
    mapping_task_t * tasks     = _get_task_base(header);
    mapping_vobj_t * vobjs     = _get_vobj_base(header);

    unsigned int taskid        = _get_context_slot(CTX_GTID_ID);

    int vobj_id = tasks[taskid].heap_vobj_id;
    if (vobj_id != -1) 
    {
        *vaddr                  = vobjs[vobj_id].vaddr;
        *size                   = vobjs[vobj_id].length;
    }
    else 
    {
        *vaddr = 0;
        *size = 0;
    }
    return 0;
}

/////////////////////////////////////////////////////////////////////////////
//      Access functions to mapping_info data structure
/////////////////////////////////////////////////////////////////////////////
inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
{
    return (mapping_cluster_t *) ((char *) header +
            MAPPING_HEADER_SIZE);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
{
    return (mapping_pseg_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
{
    return (mapping_vspace_t *)  ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
{
    return (mapping_vseg_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
{
    return (mapping_vobj_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VSEG_SIZE * header->vsegs );
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_task_t * _get_task_base(mapping_header_t * header) 
{
    return (mapping_task_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VOBJ_SIZE * header->vobjs +
            MAPPING_VSEG_SIZE * header->vsegs);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_proc_t *_get_proc_base(mapping_header_t * header) 
{
    return (mapping_proc_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VSEG_SIZE * header->vsegs +
            MAPPING_VOBJ_SIZE * header->vobjs +
            MAPPING_TASK_SIZE * header->tasks);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_irq_t *_get_irq_base(mapping_header_t * header) 
{
    return (mapping_irq_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VSEG_SIZE * header->vsegs +
            MAPPING_VOBJ_SIZE * header->vobjs +
            MAPPING_TASK_SIZE * header->tasks +
            MAPPING_PROC_SIZE * header->procs);
}
/////////////////////////////////////////////////////////////////////////////
inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
{
    return (mapping_coproc_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VOBJ_SIZE * header->vobjs +
            MAPPING_VSEG_SIZE * header->vsegs +
            MAPPING_TASK_SIZE * header->tasks +
            MAPPING_PROC_SIZE * header->procs +
            MAPPING_IRQ_SIZE * header->irqs);
}
///////////////////////////////////////////////////////////////////////////////////
inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
{
    return (mapping_cp_port_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VOBJ_SIZE * header->vobjs +
            MAPPING_VSEG_SIZE * header->vsegs +
            MAPPING_TASK_SIZE * header->tasks +
            MAPPING_PROC_SIZE * header->procs +
            MAPPING_IRQ_SIZE * header->irqs +
            MAPPING_COPROC_SIZE * header->coprocs);
}
///////////////////////////////////////////////////////////////////////////////////
inline mapping_periph_t *_get_periph_base(mapping_header_t * header) 
{
    return (mapping_periph_t *) ((char *) header +
            MAPPING_HEADER_SIZE +
            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
            MAPPING_PSEG_SIZE * header->psegs +
            MAPPING_VSPACE_SIZE * header->vspaces +
            MAPPING_VOBJ_SIZE * header->vobjs +
            MAPPING_VSEG_SIZE * header->vsegs +
            MAPPING_TASK_SIZE * header->tasks +
            MAPPING_PROC_SIZE * header->procs +
            MAPPING_IRQ_SIZE * header->irqs +
            MAPPING_COPROC_SIZE * header->coprocs +
            MAPPING_CP_PORT_SIZE * header->cp_ports);
}

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