source: soft/giet_vm/giet_libs/stdlib.c @ 795

//////////////////////////////////////////////////////////////////////////////////
// File     : stdlib.c 
// Date     : 05/12/2013
// Author   : Clément DEVIGNE
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////

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

/////////////////////////
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] == '-') //If number is negative, then update sign
    {
        sign = -1;  
        i++;           // Also update index of first digit
    }

    for (; str[i] != '\0'; ++i) // Iterate through all digits and update the result
    {
        res = res*10 + str[i] - '0';
    }

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


//////////////////////////////////////////
unsigned int xprintf( char*        string,
                      unsigned int length,
                      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++ );

        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 ('e'):
            case ('f'):
            case ('g'):             // IEEE754 64 bits
            {
                union
                {
                    double d;
                    unsigned long long ull;
                } val;
                
                val.d = va_arg( *args, double );
                
                unsigned long long digits;
                digits = val.ull & 0xFFFFFFFFFFFFFULL;    // mantisse
                
                unsigned int base;
                base = (unsigned int)((val.ull & 0x7FF0000000000000ULL) >> 52); // exp 

                unsigned int intp;
                intp = (unsigned int)val.d;         // integer part 

                unsigned int decp;                  // decimal part
                
                int isvalue = 0;
                
                if (base == 0x7FF) // special values
                {
                    if (digits & 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;
                }
                
                if (val.ull & 0x8000000000000000ULL)  // negative
                {
                    TO_STREAM( '-' );
                    val.d = val.d * -1;
                }
                else                                  // positive
                {
                    TO_STREAM( '+' );
                }
                
                if (val.d > 0xFFFFFFFF)              // overflow
                {
                    buf[0] = 'B';
                    buf[1] = 'I';
                    buf[2] = 'G';
                    len = 3;
                    pbuf = buf;
                    break;
                }
                
                val.d -= (double)intp;
                decp = (unsigned int)(val.d * 1000000000);
                
                for(i = 0; i < 10; i++) 
                {
                    if ((!isvalue) && (intp % 10)) isvalue = 1;
                    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++)
                {
                    if ((!isvalue) && (decp % 10)) isvalue = 1;
                    buf[19 - i] = HexaTab[decp % 10];
                    decp /= 10;
                }
                
                if (!isvalue)
                {
                    if (val.d != 0)                   // underflow
                    {
                        buf[0] = 'T';
                        buf[1] = 'I';
                        buf[2] = 'N';
                        buf[3] = 'Y';
                        len = 4;
                        pbuf = buf;
                    }
                }

                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()

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

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

    if ( count == 0xFFFFFFFF ) giet_pthread_exit("error in snprintf()");
    else string[count] = 0;

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