source: trunk/libs/stdlib.c @ 428

/*
 * stdlib.c - User level C library implementation.
 * 
 * Author     Alain Greiner (2016,2017)
 *
 * Copyright (c) UPMC Sorbonne Universites
 *
 * This file is part of ALMOS-MKH.
 *
 * ALMOS-MKH is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.0 of the License.
 *
 * ALMOS-MKH is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ALMOS-MKH; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <stdio.h>
#include <stdarg.h>
#include <pthread.h>
#include <stdlib.h>

/////////////////////////////
void assert( int expression ) 
{
    if( expression == 0 )
    {
        printf("\n[ASSERT FAILED] in %s at line %d in file %s\n",
        __FUNCTION__ , __LINE__ , __FILE__ );

        exit( 0 );
    }
}

//////////////////////////
int atoi(const char * str)
{
    int res  = 0;      // Initialize result
    int sign = 1;      // Initialize sign as positive
    int i    = 0;      // Initialize index of first digit

    if( (str[0] == '0') && ((str[1] == 'x') || (str[1] == 'X')) )   // hexa
    {
        i = 2;

        while( str[i] != 0 )
        {
            if     ( (str[i] >= '0') && (str[i] <= '9') ) res = (res<<4) + (str[i] - '0');
            else if( (str[i] >= 'A') && (str[i] <= 'F') ) res = (res<<4) + (str[i] - 'A');
            else if( (str[i] >= 'a') && (str[i] <= 'f') ) res = (res<<4) + (str[i] - 'a');
            else return 0;
            i++;
        }
    }
    else                                                            // decimal
    {
        if (str[0] == '-')  //  number is negative, update sign
        {
            sign = -1;  
            i++;           // Also update index of first digit
        }

        while( str[i] != 0 )
        {
            if( (str[i] >= '0') && (str[i] <= '9') ) res = (res*10) + (str[i] - '0');
            else return 0;
            i++;
        }
    }

    // Return result with sign
    return sign*res;
}

////////////////////////////
double atof(const char *str)
{
    const char *pstr = str;
    double res = 0;
    double exp = 0.1;
    short sign = 1;
    short dec = 0;

    while (*pstr != '\0')
    {
        if (*pstr == '-')
        {
            if (str != pstr) break;
            sign = -1;
        }
        
        else if (*pstr == '.')
        {
            if (dec) break;
            dec = 1;
        }
        
        else if (*pstr >= '0' && *pstr <= '9')
        {
            if (dec)
            {
                res = res + ((*pstr - '0')*exp);
                exp = exp / 10;
            }
            else
            {
                res = (res * 10) + (*pstr - '0');
            }
        }
        
        else
        {
            break;
        }
        pstr++;
    }
    return sign * res;
}

///////////////////////////////////////////////////////////////
void * memcpy(void *_dst, const void * _src, unsigned int size) 
{
    unsigned int * dst = _dst;
    const unsigned int * src = _src;
    if (!((unsigned int) dst & 3) && !((unsigned int) src & 3) )
    {
        while (size > 3) 
        {
            *dst++ = *src++;
            size -= 4;
        }
    }

    unsigned char *cdst = (unsigned char*)dst;
    unsigned char *csrc = (unsigned char*)src;

    while (size--) 
    {
        *cdst++ = *csrc++;
    }
    return _dst;
}

//////////////////////////////////////////////////////////
inline void * memset(void * dst, int s, unsigned int size) 
{
    char * a = (char *) dst;
    while (size--)
    {
        *a++ = (char)s;
    }
    return dst;
}


//////////////////////////////////////////
static int xprintf( char         * string,
                    unsigned int   length,
                    const char   * format, 
                    va_list      * args ) 
{
    unsigned int ps = 0;    // write pointer to the string buffer

#define TO_STREAM(x) do { string[ps] = (x); ps++; if(ps==length) return -1; } while(0);

xprintf_text:

    while ( *format != 0 ) 
    {

        if (*format == '%')   // copy argument to string
        {
            format++;
            goto xprintf_arguments;
        }
        else                  // copy one char to string
        {
            TO_STREAM( *format );
            format++;
        }
    }

    return ps;

xprintf_arguments:

    {
        char              buf[30];    // buffer to display one number
        char *            pbuf;       // pointer on first char to display
        unsigned int      len = 0;    // number of char to display
        static const char HexaTab[] = "0123456789ABCDEF";
        unsigned int      i;
        
        // Ignore fields width and precision 
        for ( ; (*format >= '0' && *format <= '9') || (*format == '.') ; format++ );

        switch (*format) 
        {
            case ('c'):             // char conversion 
            {
                int val = va_arg( *args, int );
                buf[0] = val;
                pbuf   = buf;
                len    = 1;
                break;
            }
            case ('d'):             // decimal signed integer
            {
                int val = va_arg( *args, int );
                if (val < 0) 
                {
                    TO_STREAM( '-' );
                    val = -val;
                }
                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'):             // decimal unsigned integer
            {
                unsigned int val = va_arg( *args, 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 
            case ('l'):             // 64 bits hexadecimal 
            {
                unsigned int       imax;
                unsigned long long val;
                
                if ( *format == 'l' )   // 64 bits
                {
                    val = va_arg( *args, unsigned long long);
                    imax = 16;
                }
                else                    // 32 bits
                {
                    val = va_arg( *args, unsigned int);
                    imax = 8;
                }
                
                TO_STREAM( '0' );
                TO_STREAM( 'x' );
                
                for(i = 0; i < imax; i++) 
                {
                    buf[(imax-1) - i] = HexaTab[val % 16];
                    if (!(val /= 16))  break;
                }
                len =  i + 1;
                pbuf = &buf[(imax-1) - i];
                break;
            }
            case ('s'):             /* string */
            {
                char* str = va_arg( *args, char* );
                while (str[len]) { len++; }
                pbuf = str;
                break;
            }
/*
            case ('f'):             // IEEE754 64 bits
                                    // integer part : up to 10 decimal digits
                                    // decimal part : 9 decimal digits
            {
                union
                {
                    double d;
                    unsigned long long ull;
                } val;
                
                val.d = va_arg( *args, double );
                
                unsigned long long mantisse;
                mantisse = val.ull & 0xFFFFFFFFFFFFFULL;    // mantisse
                
                unsigned int exp;
                exp = (unsigned int)((val.ull & 0x7FF0000000000000ULL) >> 52); // exp 

                if (exp == 0x7FF) // special values
                {
                    if (mantisse & 0xFFFFFFFFFFFFFULL)  // Not a Number
                    {
                        buf[0] = 'N';
                        buf[1] = 'a';
                        buf[2] = 'N';
                        len = 3;
                        pbuf = buf;
                    }
                    else                              // infinite
                    {
                        // inf
                        buf[0] = (val.ull & 0x8000000000000000ULL) ? '-' : '+';
                        buf[1] = 'i';
                        buf[2] = 'n';
                        buf[3] = 'f';
                        len = 4;
                        pbuf = buf;
                    }
                    break;
                }

                // display sign & analyse overflow 
                unsigned int overflow = 0;
                if (val.ull & 0x8000000000000000ULL)  // negative 
                {
                    TO_STREAM( '-' );
                    val.d = val.d * -1;
                    if( val.d < -9999999999.0) overflow = 1;
                }
                else                                  // positive
                {
                    TO_STREAM( '+' );
                    if( val.d > 9999999999.0) overflow = 1;
                }
                
                // check overflow caused by the 10.9 format
                if ( overflow )    
                {
                    buf[0] = 'o';
                    buf[1] = 'v';
                    buf[2] = 'r';
                    len = 3;
                    pbuf = buf;
                    break;
                }

                // compute integer & decimal parts
                unsigned int intp;                  // integer part
                unsigned int decp;                  // decimal part
                intp = (unsigned int)val.d;      
                val.d -= (double)intp;
                decp = (unsigned int)(val.d * 1000000000);
                
                // display decimal value in 10.9 format
                for(i = 0; i < 10; i++) 
                {
                    buf[9 - i] = HexaTab[intp % 10];
                    if (!(intp /= 10)) break;
                }
                pbuf = &buf[9 - i];
                len = i+11;
                buf[10] = '.';
                for(i = 0; i < 9; i++)
                {
                    buf[19 - i] = HexaTab[decp % 10];
                    decp /= 10;
                }
                break;
            }
*/                    
            default:       // unsupported argument type
            {
                return -1;
            }
        }  // end switch on  argument type

        format++;

        // copy argument to string
        for( i = 0 ; i < len ; i++ )
        {
            TO_STREAM( pbuf[i] );
        }
        
        goto xprintf_text;
    }
} // end xprintf()

//////////////////////////////////////
int printf( const char * format, ... )
{
    char      string[4096];
    va_list   args;
    int       count;
    
    va_start( args, format );
    count = xprintf( string , 4095 , format , &args ); 
    va_end( args );

    if ( count == -1 ) 
    {
        display_string( "stdlib : xprintf failure" );
        return -1;
    }
    else 
    {
        string[count] = 0;
        return write( 1 , &string , count + 1 );
    }
}

/////////////
int getchar()
{
    char byte;

    if ( read( 0 , &byte , 1 ) != 1 ) return 0;
    else                              return (int)byte; 
}

////////////////////
int putchar( int c )
{
    char byte = (char)c;

    if( write( 1 , &byte , 1 ) != 1 ) return 0;
    else                              return c; 
}

////////////
int getint()
{
    unsigned int  i;
    int           val;    // ASCII character value

    unsigned char buf[32];
    unsigned int  save          = 0;
    unsigned int  dec           = 0;
    unsigned int  done          = 0;
    unsigned int  overflow      = 0;
    unsigned int  length        = 0;
 
    // get characters
    while (done == 0) 
    {
        // read one character
        val = getchar();

        // analyse character
        if ((val > 0x2F) && (val < 0x3A))         // decimal character 
        {
            buf[length] = (unsigned char)val;
            length++;                      
            putchar( val );                       // echo
        }
        else if (val == 0x0A)                     // LF character 
        {
            done = 1;
        }
        else if ( (val == 0x7F) ||                // DEL character
                  (val == 0x08) )                 // BS  character 
        {
            if ( length > 0 ) 
            {
                length--;          
                printf("\b \b");                  // BS /  / BS 
            }
        }
        else if ( val == 0 )                      // EOF
        {
            return -1;
        }

        // test buffer overflow
        if ( length >= 32 )  
        {
            overflow = 1;
            done     = 1;
        }
    }  // end while characters

    // string to int conversion with overflow detection 
    if ( overflow == 0 )
    {
        for (i = 0; (i < length) && (overflow == 0) ; i++) 
        {
            dec = dec * 10 + (buf[i] - 0x30);
            if (dec < save)  overflow = 1; 
            save = dec;
        }
    } 

    // final evaluation 
    if ( overflow == 0 )
    {
        // return value
        return dec;
    }
    else
    {
        // cancel all echo characters
        for (i = 0; i < length ; i++) 
        {
            printf("\b \b");                  // BS /  / BS 
        }

        // echo character '0'
        putchar( '0' );

        // return 0 value 
        return 0;
    }
}  // end getint()

///////////////////////////////////////
int snprintf( char           * string,
              unsigned int     length,
              const char     * format, ... )
{
    va_list   args;
    int       count;

    va_start( args, format );
    count = xprintf( string , length , format , &args ); 
    va_end( args );

    if( count < length ) string[count] = 0;

    return count;
}

//////////
int rand()
{
    unsigned long long cycle;

    get_cycle( &cycle );

    unsigned int x = (unsigned int)cycle;

    if ((x & 0xF) > 7) 
    {
        return (x*x & 0xFFFF);
    }
    else 
    {
        return (x*x*x & 0xFFFF);
    }
}

///////////////////////////////
void srand( unsigned int seed )
{
    printf("\n[ERROR] in %s : not implemented yet : do nothing\n", __FUNCTION__ );
}