source: soft/giet_vm/giet_libs/stdlib.c

Last change on this file was 810, checked in by alain, 8 years ago

Fix a bug in xprintf() function for %f format.
File size: 10.2 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 ('f'):             // IEEE754 64 bits
233                                    // integer part : up to 10 decimal digits
234                                    // decimal part : 9 decimal digits
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 mantisse;
245                mantisse = val.ull & 0xFFFFFFFFFFFFFULL;    // mantisse
246               
247                unsigned int exp;
248                exp = (unsigned int)((val.ull & 0x7FF0000000000000ULL) >> 52); // exp
249
250                if (exp == 0x7FF) // special values
251                {
252                    if (mantisse & 0xFFFFFFFFFFFFFULL)  // Not a Number
253                    {
254                        buf[0] = 'N';
255                        buf[1] = 'a';
256                        buf[2] = 'N';
257                        len = 3;
258                        pbuf = buf;
259                    }
260                    else                              // infinite
261                    {
262                        /* inf */
263                        buf[0] = (val.ull & 0x8000000000000000ULL) ? '-' : '+';
264                        buf[1] = 'i';
265                        buf[2] = 'n';
266                        buf[3] = 'f';
267                        len = 4;
268                        pbuf = buf;
269                    }
270                    break;
271                }
272
273                // display sign & analyse overflow
274                unsigned int overflow = 0;
275                if (val.ull & 0x8000000000000000ULL)  // negative
276                {
277                    TO_STREAM( '-' );
278                    val.d = val.d * -1;
279                    if( val.d < -9999999999.0) overflow = 1;
280                }
281                else                                  // positive
282                {
283                    TO_STREAM( '+' );
284                    if( val.d > 9999999999.0) overflow = 1;
285                }
286               
287                // check overflow caused by the 10.9 format
288                if ( overflow )   
289                {
290                    buf[0] = 'o';
291                    buf[1] = 'v';
292                    buf[2] = 'r';
293                    len = 3;
294                    pbuf = buf;
295                    break;
296                }
297
298                // compute integer & decimal parts
299                unsigned int intp;                  // integer part
300                unsigned int decp;                  // decimal part
301                intp = (unsigned int)val.d;     
302                val.d -= (double)intp;
303                decp = (unsigned int)(val.d * 1000000000);
304               
305                // display decimal value in 10.9 format
306                for(i = 0; i < 10; i++) 
307                {
308                    buf[9 - i] = HexaTab[intp % 10];
309                    if (!(intp /= 10)) break;
310                }
311                pbuf = &buf[9 - i];
312                len = i+11;
313                buf[10] = '.';
314                for(i = 0; i < 9; i++)
315                {
316                    buf[19 - i] = HexaTab[decp % 10];
317                    decp /= 10;
318                }
319                break;
320            }
321                   
322            default:       // unsupported argument type
323            {
324                return -1;
325            }
326        }  // end switch on  argument type
327
328        format++;
329
330        // copy argument to string
331        for( i = 0 ; i < len ; i++ )
332        {
333            TO_STREAM( pbuf[i] );
334        }
335       
336        goto xprintf_text;
337    }
338} // end xprintf()
339
340///////////////////////////////////////////
341unsigned int snprintf( char*        string,
342                       unsigned int length,
343                       char*        format, ... )
344{
345    va_list      args;
346    unsigned int count;
347
348    va_start( args, format );
349    count = xprintf( string , length , format , &args ); 
350    va_end( args );
351
352    if ( count == 0xFFFFFFFF ) giet_pthread_exit("error in snprintf()");
353    else string[count] = 0;
354
355    return count;
356}
357// Local Variables:
358// tab-width: 4
359// c-basic-offset: 4
360// c-file-offsets:((innamespace . 0)(inline-open . 0))
361// indent-tabs-mode: nil
362// End:
363// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
Note: See TracBrowser for help on using the repository browser.