///////////////////////////////////////////////////////////////////////////////////
// File     : kernel_utils.c
// Date     : 01/11/2014
// Author   : alain greiner
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////

#include <kernel_utils.h>
#include <sys_handler.h>
#include <hard_config.h>
#include <utils.h>
#include <stdarg.h>

//////////////////////////////////
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
    _sys_tty_get_lock( 0, &save_sr );

printf_text:

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

    // release TTY0 lock
    _sys_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 ( _sys_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 ( _sys_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 ( _sys_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 ( _sys_tty_write( pbuf, len, 0 ) != len ) goto return_error;
        
        goto printf_text;
    }

return_error:

    {
        unsigned int procid     = _get_procid();
        unsigned int x          = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
        unsigned int y          = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
        unsigned int lpid       = procid & ((1<<P_WIDTH)-1);

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

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

        _exit();
    }
}  // end _printf()

///////////////////////////////////////////////////
unsigned int _atomic_increment( unsigned int* ptr )
{
    unsigned int value;

    asm volatile (
        "_atomic_increment_try:             \n"
        "ll   $3,    0(%1)                  \n"
        "move %0,    $3                     \n"
        "addi $3,    $3,     1              \n"     
        "sc   $3,    0(%1)                  \n"
        "beqz $3,    _atomic_increment_try  \n"
        : "=r" (value) 
        : "r" (ptr)
        : "$3", "memory" );

    return value;
}

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