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

Last change on this file since 802 was 801, checked in by alain, 9 years ago

cosmetic.
File size: 10.4 KB
Line 
1//////////////////////////////////////////////////////////////////////////////////
2// File     : stdlib.c
3// Date     : 05/12/2013
4// Author   : Clément DEVIGNE
5// Copyright (c) UPMC-LIP6
6///////////////////////////////////////////////////////////////////////////////////
7
8#include <stdlib.h>
9#include <stdio.h>
10#include <stdarg.h>
11
12/////////////////////////
13int atoi(const char *str)
14{
15    int res  = 0; // Initialize result
16    int sign = 1; // Initialize sign as positive
17    int i    = 0; // Initialize index of first digit
18
19    if (str[0] == '-') //If number is negative, then update sign
20    {
21        sign = -1; 
22        i++;           // Also update index of first digit
23    }
24
25    for (; str[i] != '\0'; ++i) // Iterate through all digits and update the result
26    {
27        res = res*10 + str[i] - '0';
28    }
29
30    // Return result with sign
31    return sign*res;
32}
33
34////////////////////////////
35double atof(const char *str)
36{
37    const char *pstr = str;
38    double res = 0;
39    double exp = 0.1;
40    short sign = 1;
41    short dec = 0;
42
43    while (*pstr != '\0')
44    {
45        if (*pstr == '-')
46        {
47            if (str != pstr) break;
48            sign = -1;
49        }
50       
51        else if (*pstr == '.')
52        {
53            if (dec) break;
54            dec = 1;
55        }
56       
57        else if (*pstr >= '0' && *pstr <= '9')
58        {
59            if (dec)
60            {
61                res = res + ((*pstr - '0')*exp);
62                exp = exp / 10;
63            }
64            else
65            {
66                res = (res * 10) + (*pstr - '0');
67            }
68        }
69       
70        else
71        {
72            break;
73        }
74        pstr++;
75    }
76    return sign * res;
77}
78
79///////////////////////////////////////////////////////////////
80void * memcpy(void *_dst, const void * _src, unsigned int size) 
81{
82    unsigned int * dst = _dst;
83    const unsigned int * src = _src;
84    if (!((unsigned int) dst & 3) && !((unsigned int) src & 3) )
85    {
86        while (size > 3) 
87        {
88            *dst++ = *src++;
89            size -= 4;
90        }
91    }
92
93    unsigned char *cdst = (unsigned char*)dst;
94    unsigned char *csrc = (unsigned char*)src;
95
96    while (size--) 
97    {
98        *cdst++ = *csrc++;
99    }
100    return _dst;
101}
102
103//////////////////////////////////////////////////////////
104inline void * memset(void * dst, int s, unsigned int size) 
105{
106    char * a = (char *) dst;
107    while (size--)
108    {
109        *a++ = (char)s;
110    }
111    return dst;
112}
113
114
115//////////////////////////////////////////
116unsigned int xprintf( char*        string,
117                      unsigned int length,
118                      char*        format, 
119                      va_list*     args ) 
120{
121    unsigned int ps = 0;    // write pointer to the string buffer
122
123#define TO_STREAM(x) do { string[ps] = (x); ps++; if(ps==length) return -1; } while(0);
124
125xprintf_text:
126
127    while ( *format != 0 ) 
128    {
129
130        if (*format == '%')   // copy argument to string
131        {
132            format++;
133            goto xprintf_arguments;
134        }
135        else                  // copy one char to string
136        {
137            TO_STREAM( *format );
138            format++;
139        }
140    }
141
142    return ps;
143
144xprintf_arguments:
145
146    {
147        char              buf[30];    // buffer to display one number
148        char *            pbuf;       // pointer on first char to display
149        unsigned int      len = 0;    // number of char to display
150        static const char HexaTab[] = "0123456789ABCDEF";
151        unsigned int      i;
152       
153        // Ignore fields width and precision
154        for ( ; (*format >= '0' && *format <= '9') || (*format == '.') ; format++ );
155
156        switch (*format) 
157        {
158            case ('c'):             // char conversion
159            {
160                int val = va_arg( *args, int );
161                buf[0] = val;
162                pbuf   = buf;
163                len    = 1;
164                break;
165            }
166            case ('d'):             // decimal signed integer
167            {
168                int val = va_arg( *args, int );
169                if (val < 0) 
170                {
171                    TO_STREAM( '-' );
172                    val = -val;
173                }
174                for(i = 0; i < 10; i++) 
175                {
176
177                    buf[9 - i] = HexaTab[val % 10];
178                    if (!(val /= 10)) break;
179                }
180                len =  i + 1;
181                pbuf = &buf[9 - i];
182                break;
183            }
184            case ('u'):             // decimal unsigned integer
185            {
186                unsigned int val = va_arg( *args, unsigned int );
187                for(i = 0; i < 10; i++) 
188                {
189                    buf[9 - i] = HexaTab[val % 10];
190                    if (!(val /= 10)) break;
191                }
192                len =  i + 1;
193                pbuf = &buf[9 - i];
194                break;
195            }
196            case ('x'):             // 32 bits hexadecimal
197            case ('l'):             // 64 bits hexadecimal
198            {
199                unsigned int       imax;
200                unsigned long long val;
201               
202                if ( *format == 'l' )   // 64 bits
203                {
204                    val = va_arg( *args, unsigned long long);
205                    imax = 16;
206                }
207                else                    // 32 bits
208                {
209                    val = va_arg( *args, unsigned int);
210                    imax = 8;
211                }
212               
213                TO_STREAM( '0' );
214                TO_STREAM( 'x' );
215               
216                for(i = 0; i < imax; i++) 
217                {
218                    buf[(imax-1) - i] = HexaTab[val % 16];
219                    if (!(val /= 16))  break;
220                }
221                len =  i + 1;
222                pbuf = &buf[(imax-1) - i];
223                break;
224            }
225            case ('s'):             /* string */
226            {
227                char* str = va_arg( *args, char* );
228                while (str[len]) { len++; }
229                pbuf = str;
230                break;
231            }
232            case ('e'):
233            case ('f'):
234            case ('g'):             // IEEE754 64 bits
235            {
236                union
237                {
238                    double d;
239                    unsigned long long ull;
240                } val;
241               
242                val.d = va_arg( *args, double );
243               
244                unsigned long long digits;
245                digits = val.ull & 0xFFFFFFFFFFFFFULL;    // mantisse
246               
247                unsigned int base;
248                base = (unsigned int)((val.ull & 0x7FF0000000000000ULL) >> 52); // exp
249
250                unsigned int intp;
251                intp = (unsigned int)val.d;         // integer part
252
253                unsigned int decp;                  // decimal part
254               
255                int isvalue = 0;
256               
257                if (base == 0x7FF) // special values
258                {
259                    if (digits & 0xFFFFFFFFFFFFFULL)  // Not a Number
260                    {
261                        buf[0] = 'N';
262                        buf[1] = 'a';
263                        buf[2] = 'N';
264                        len = 3;
265                        pbuf = buf;
266                    }
267                    else                              // infinite
268                    {
269                        /* inf */
270                        buf[0] = (val.ull & 0x8000000000000000ULL) ? '-' : '+';
271                        buf[1] = 'i';
272                        buf[2] = 'n';
273                        buf[3] = 'f';
274                        len = 4;
275                        pbuf = buf;
276                    }
277                    break;
278                }
279               
280                if (val.ull & 0x8000000000000000ULL)  // negative
281                {
282                    TO_STREAM( '-' );
283                    val.d = val.d * -1;
284                }
285                else                                  // positive
286                {
287                    TO_STREAM( '+' );
288                }
289               
290                if (val.d > 0xFFFFFFFF)              // overflow
291                {
292                    buf[0] = 'B';
293                    buf[1] = 'I';
294                    buf[2] = 'G';
295                    len = 3;
296                    pbuf = buf;
297                    break;
298                }
299               
300                val.d -= (double)intp;
301                decp = (unsigned int)(val.d * 1000000000);
302               
303                for(i = 0; i < 10; i++) 
304                {
305                    if ((!isvalue) && (intp % 10)) isvalue = 1;
306                    buf[9 - i] = HexaTab[intp % 10];
307                    if (!(intp /= 10)) break;
308                }
309                pbuf = &buf[9 - i];
310                len = i+11;
311                buf[10] = '.';
312               
313                for(i = 0; i < 9; i++)
314                {
315                    if ((!isvalue) && (decp % 10)) isvalue = 1;
316                    buf[19 - i] = HexaTab[decp % 10];
317                    decp /= 10;
318                }
319               
320                if (!isvalue)
321                {
322                    if (val.d != 0)                   // underflow
323                    {
324                        buf[0] = 'T';
325                        buf[1] = 'I';
326                        buf[2] = 'N';
327                        buf[3] = 'Y';
328                        len = 4;
329                        pbuf = buf;
330                    }
331                }
332
333                break;
334            }
335                   
336            default:       // unsupported argument type
337            {
338                return -1;
339            }
340        }  // end switch on  argument type
341
342        format++;
343
344        // copy argument to string
345        for( i = 0 ; i < len ; i++ )
346        {
347            TO_STREAM( pbuf[i] );
348        }
349       
350        goto xprintf_text;
351    }
352} // end xprintf()
353
354///////////////////////////////////////////
355unsigned int snprintf( char*        string,
356                       unsigned int length,
357                       char*        format, ... )
358{
359    va_list      args;
360    unsigned int count;
361
362    va_start( args, format );
363    count = xprintf( string , length , format , &args ); 
364    va_end( args );
365
366    if ( count == 0xFFFFFFFF ) giet_pthread_exit("error in snprintf()");
367    else string[count] = 0;
368
369    return count;
370}
371// Local Variables:
372// tab-width: 4
373// c-basic-offset: 4
374// c-file-offsets:((innamespace . 0)(inline-open . 0))
375// indent-tabs-mode: nil
376// End:
377// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
Note: See TracBrowser for help on using the repository browser.