bd67e00fe8205b977b5a27ff2e29a240fa236d5f
[tpg/acess2.git] / Usermode / Libraries / libc.so_src / printf.c
1 /*
2  * Acess2 C Library
3  * - By John Hodge (thePowersGang)
4  *
5  * scanf.c
6  * - *scanf family of functions
7  */
8 #include "lib.h"
9 #include <stdio.h>
10 #include <stdarg.h>
11 #include <string.h>
12 #include <ctype.h>      // toupper
13 #include <assert.h>     // assert() 
14
15 // Quick booleans
16 typedef char    BOOL;
17 #define TRUE    1
18 #define FALSE   0
19
20 // === TYPES ===
21 typedef void    (*printf_putch_t)(void *h, char ch);
22 enum eFPN {
23         FPN_STD,
24         FPN_SCI,
25         FPN_SHORTEST,
26 };
27
28 // === PROTOTYPES ===
29 void    itoa(char *buf, uint64_t num, size_t base, int minLength, char pad, int bSigned);
30 size_t  _printf_itoa(printf_putch_t putch_cb, void *putch_h, uint64_t num,
31         size_t base, int bUpper,
32         int bSigned, char SignChar, int Precision,
33         int PadLength, char PadChar, int bPadRight);
34 size_t  _printf_ftoa_hex(printf_putch_t putch_cb, void *putch_h, long double num, int Precision, int bForcePoint, int bForceSign, int bCapitals);
35 size_t  _printf_ftoa(printf_putch_t putch_cb, void *putch_h, long double num, size_t Base, enum eFPN Notation, int Precision, int bForcePoint, int bForceSign, int bCapitals);
36
37 // === CODE ===
38 /**
39  * \fn EXPORT void vsnprintf(char *buf, const char *format, va_list args)
40  * \brief Prints a formatted string to a buffer
41  * \param buf   Pointer - Destination Buffer
42  * \param format        String - Format String
43  * \param args  VarArgs List - Arguments
44  */
45 EXPORT int _vcprintf_int(printf_putch_t putch_cb, void *putch_h, const char *format, va_list args)
46 {
47         char    tmp[65];
48          int    c, minSize, precision, len;
49         size_t  pos = 0;
50         char    *p;
51         uint64_t        arg;
52         long double     arg_f;
53 //      char    cPositiveChar;
54         BOOL    bLongLong, bLong, bJustifyLeft, bAltForm;
55         char    cNumPad, cPlus;
56
57         #define _addchar(ch) do { \
58                 putch_cb(putch_h, ch); \
59                 pos ++; \
60         } while(0)
61
62         tmp[32] = '\0';
63         
64         while((c = *format++) != 0)
65         {
66                 // Non-control character
67                 if (c != '%') {
68                         _addchar(c);
69                         continue;
70                 }
71                 
72                 // Control Character
73                 c = *format++;
74                 if(c == '%') {  // Literal %
75                         _addchar('%');
76                         continue;
77                 }
78                 
79                 bAltForm = 0;
80                 cNumPad = ' ';
81                 bJustifyLeft = 0;
82                 cPlus = '\0';
83                 bLong = 0;
84                 bLongLong = 0;
85                 minSize = 0;
86                 precision = -1;
87                 
88                 // - Flags
89                 do
90                 {
91                         // Alternate form (0<oct>, 0x<hex>, 123.)
92                         if(c == '#') {
93                                 bAltForm = 1;
94                         }
95                         // Padding with '0'
96                         else if(c == '0') {
97                                 cNumPad = '0';
98                         }
99                         // Pad on left
100                         else if(c == '-') {
101                                 bJustifyLeft = 1;
102                         }
103                         // Include space for positive sign
104                         else if(c == ' ') {
105                                 cPlus = ' ';
106                         }
107                         // Always include sign
108                         else if(c == '+') {
109                                 cPlus = '+';
110                         }
111                         else
112                                 break;
113                 }
114                 while( (c = *format++) );
115                 
116                 // Padding length
117                 if( c == '*' ) {
118                         // Variable length
119                         minSize = va_arg(args, size_t);
120                         c = *format++;
121                 }
122                 else if('1' <= c && c <= '9')
123                 {
124                         minSize = 0;
125                         while('0' <= c && c <= '9')
126                         {
127                                 minSize *= 10;
128                                 minSize += c - '0';
129                                 c = *format++;
130                         }
131                 }
132
133                 // Precision
134                 if(c == '.') {
135                         c = *format++;
136                         if(c == '*') {
137                                 precision = va_arg(args, size_t);
138                                 c = *format++;
139                         }
140                         else if('1' <= c && c <= '9')
141                         {
142                                 precision = 0;
143                                 while('0' <= c && c <= '9')
144                                 {
145                                         precision *= 10;
146                                         precision += c - '0';
147                                         c = *format++;
148                                 }
149                         }
150                 }
151         
152                 // Check for long long
153                 if(c == 'l')
154                 {
155                         bLong = 1;
156                         c = *format++;
157                         if(c == 'l') {
158                                 bLongLong = 1;
159                                 c = *format++;
160                         }
161                 }
162                 
163                 // Just help things along later
164                 p = tmp;
165                 
166                 // Get Type
167                 switch( c )
168                 {
169                 // Signed Integer
170                 case 'd':
171                 case 'i':
172                         // Get Argument
173                         arg = bLongLong ? va_arg(args, int64_t) : va_arg(args, int32_t);
174                         if( arg == 0 && precision == 0 )
175                                 break;
176                         pos += _printf_itoa(putch_cb, putch_h, arg, 10, FALSE,
177                                 TRUE, cPlus, precision, minSize, cNumPad, bJustifyLeft);
178                         break;
179                 
180                 // Unsigned Integer
181                 case 'u':
182                         arg = bLongLong ? va_arg(args, int64_t) : va_arg(args, int32_t);
183                         pos += _printf_itoa(putch_cb, putch_h, arg, 10, FALSE,
184                                 FALSE, '\0', precision, minSize, cNumPad, bJustifyLeft);
185                         break;
186                 
187                 // Pointer
188                 case 'p':
189                         _addchar('*');
190                         _addchar('0');
191                         _addchar('x');
192                         arg = va_arg(args, intptr_t);
193                         pos += _printf_itoa(putch_cb, putch_h, arg, 16, FALSE,
194                                 FALSE, '\0', sizeof(intptr_t)*2, 0,'\0',FALSE);
195                         break;
196                 // Unsigned Hexadecimal
197                 case 'X':
198                 case 'x':
199                         if(bAltForm) {
200                                 _addchar('0');
201                                 _addchar(c);
202                         }
203                         arg = bLongLong ? va_arg(args, int64_t) : va_arg(args, int32_t);
204                         pos += _printf_itoa(putch_cb, putch_h, arg, 16, c=='X',
205                                 FALSE, '\0', precision, minSize,cNumPad,bJustifyLeft);
206                         break;
207                 
208                 // Unsigned Octal
209                 case 'o':
210                         if(bAltForm) {
211                                 _addchar('0');
212                         }
213                         arg = bLongLong ? va_arg(args, int64_t) : va_arg(args, int32_t);
214                         pos += _printf_itoa(putch_cb, putch_h, arg, 8, FALSE,
215                                 FALSE, '\0', precision, minSize,cNumPad,bJustifyLeft);
216                         break;
217                 
218                 // Unsigned binary
219                 case 'b':
220                         if(bAltForm) {
221                                 _addchar('0');
222                                 _addchar('b');
223                         }
224                         arg = bLongLong ? va_arg(args, int64_t) : va_arg(args, int32_t);
225                         pos += _printf_itoa(putch_cb, putch_h, arg, 2, FALSE,
226                                 FALSE, '\0', precision, minSize,cNumPad,bJustifyLeft);
227                         break;
228
229                 // Standard float
230                 case 'f':
231                 case 'F':
232                         arg_f = bLong ? va_arg(args, long double) : va_arg(args, double);
233                         pos += _printf_ftoa(putch_cb, putch_h, arg_f, 10, FPN_STD,
234                                 precision, 0, bJustifyLeft, c == 'F');
235                         break;
236                 // Scientific Float
237                 case 'e':
238                 case 'E':
239                         arg_f = bLong ? va_arg(args, long double) : va_arg(args, double);
240                         pos += _printf_ftoa(putch_cb, putch_h, arg_f, 10, FPN_SCI,
241                                 precision, 0, bJustifyLeft, c == 'E');
242                         break;
243                 // Scientific Float
244                 case 'g':
245                 case 'G':
246                         arg_f = bLong ? va_arg(args, long double) : va_arg(args, double);
247                         pos += _printf_ftoa(putch_cb, putch_h, arg_f, 10, FPN_SHORTEST,
248                                 precision, 0, bJustifyLeft, c == 'G');
249                         break;
250                 // Hexadecimal Scientific
251                 case 'a':
252                 case 'A':
253                         arg_f = bLong ? va_arg(args, long double) : va_arg(args, double);
254                         pos += _printf_ftoa_hex(putch_cb, putch_h, arg_f, precision, 0, bJustifyLeft, c == 'A');
255                         break;
256
257                 // String
258                 case 's':
259                         p = va_arg(args, char*);
260                         if(!p)  p = "(null)";
261                         //_SysDebug("vsnprintf: p = '%s'", p);
262                         if(precision >= 0)
263                                 len = strnlen(p, precision);
264                         else
265                                 len = strlen(p);
266                         if(!bJustifyLeft)
267                                 while(minSize > len++)  _addchar(' ');
268                         while( *p ) {
269                                 if(precision >= 0 && precision -- == 0)
270                                         break;
271                                 _addchar(*p++);
272                         }
273                         if(bJustifyLeft)
274                                 while(minSize > len++)  _addchar(' ');
275                         break;
276
277                 // Unknown, just treat it as a character
278                 default:
279                         arg = va_arg(args, uint32_t);
280                         _addchar(arg);
281                         break;
282                 }
283         }
284         #undef _addchar
285         
286         return pos;
287 }
288
289 struct s_sprintf_info {
290         char    *dest;
291         size_t  ofs;
292         size_t  maxlen;
293 };
294
295 void _vsnprintf_putch(void *h, char ch)
296 {
297         struct s_sprintf_info   *info = h;
298         if(info->ofs < info->maxlen)
299                 info->dest[info->ofs++] = ch;
300 }
301
302 EXPORT int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list __args)
303 {
304         struct s_sprintf_info   info = {__s, 0, __maxlen};
305         int ret;
306         ret = _vcprintf_int(_vsnprintf_putch, &info, __format, __args);
307         _vsnprintf_putch(&info, '\0');
308         return ret;
309 }
310
311 EXPORT int snprintf(char *buf, size_t maxlen, const char *format, ...)
312 {
313          int    ret;
314         va_list args;
315         va_start(args, format);
316         ret = vsnprintf((char*)buf, maxlen, (char*)format, args);
317         va_end(args);
318         return ret;
319 }
320
321
322 EXPORT int vsprintf(char * __s, const char *__format, va_list __args)
323 {
324         return vsnprintf(__s, 0x7FFFFFFF, __format, __args);
325 }
326 EXPORT int sprintf(char *buf, const char *format, ...)
327 {
328          int    ret;
329         va_list args;
330         va_start(args, format);
331         ret = vsprintf((char*)buf, (char*)format, args);
332         va_end(args);
333         return ret;
334 }
335
336 void _vfprintf_putch(void *h, char ch)
337 {
338         fputc(ch, h);
339 }
340
341 EXPORT int vfprintf(FILE *__fp, const char *__format, va_list __args)
342 {
343         return _vcprintf_int(_vfprintf_putch, __fp, __format, __args);
344 }
345
346 EXPORT int fprintf(FILE *fp, const char *format, ...)
347 {
348         va_list args;
349          int    ret;
350         
351         // Get Size
352         va_start(args, format);
353         ret = vfprintf(fp, (char*)format, args);
354         va_end(args);
355         
356         return ret;
357 }
358
359 EXPORT int vprintf(const char *__format, va_list __args)
360 {
361         return vfprintf(stdout, __format, __args);
362 }
363
364 EXPORT int printf(const char *format, ...)
365 {
366         va_list args;
367          int    ret;
368         
369         // Get final size
370         va_start(args, format);
371         ret = vprintf(format, args);
372         va_end(args);
373         
374         // Return
375         return ret;
376 }
377
378 void itoa(char *buf, uint64_t num, size_t base, int minLength, char pad, int bSigned)
379 {
380         struct s_sprintf_info   info = {buf, 0, 1024};
381         if(!buf)        return;
382         _printf_itoa(_vsnprintf_putch, &info, num, base, FALSE, bSigned, '\0', 0, minLength, pad, FALSE);
383         buf[info.ofs] = '\0';
384 }
385
386 const char cDIGITS[] = "0123456789abcdef";
387 const char cUDIGITS[] = "0123456789ABCDEF";
388 /**
389  * \brief Convert an integer into a character string
390  * \param buf   Destination Buffer
391  * \param num   Number to convert
392  * \param base  Base-n number output
393  * \param minLength     Minimum length of output
394  * \param pad   Padding used to ensure minLength
395  * \param bSigned       Signed number output?
396  */
397 size_t _printf_itoa(printf_putch_t putch_cb, void *putch_h, uint64_t num,
398         size_t base, int bUpper,
399         int bSigned, char SignChar, int Precision,
400         int PadLength, char PadChar, int bPadRight)
401 {
402         char    tmpBuf[64];
403          int    pos = 0;
404         size_t  ret = 0;
405          int    sign_is_neg = 0;
406         const char *map = bUpper ? cUDIGITS : cDIGITS;
407
408         if(base > 16 || base < 2) {
409                 return 0;
410         }
411         
412         if(bSigned && (int64_t)num < 0)
413         {
414                 num = -(int64_t)num;
415                 sign_is_neg = 1;
416         }
417         
418         // Encode into reversed string
419         while(num > base-1) {
420                 tmpBuf[pos++] = map[ num % base ];
421                 num = (uint64_t) num / (uint64_t)base;          // Shift {number} right 1 digit
422         }
423
424         tmpBuf[pos++] = map[ num % base ];              // Last digit of {number}
425         
426         // length of number, minus the sign character
427         PadLength -= pos + (sign_is_neg || SignChar != '\0');
428         Precision -= pos + (sign_is_neg || SignChar != '\0');
429         if( !bPadRight )
430         {
431                 while(PadLength-- > 0)
432                         putch_cb(putch_h, PadChar), ret ++;
433         }
434         
435         if(sign_is_neg)
436                 putch_cb(putch_h, '-'), ret++;  // Negative sign character
437         else if(SignChar)
438                 putch_cb(putch_h, SignChar), ret++;     // positive sign character
439         else {
440         }
441         
442         while( Precision-- > 0 )
443                 putch_cb(putch_h, '0'), ret++;
444         while(pos--)
445                 putch_cb(putch_h, tmpBuf[pos]), ret++;  // Reverse the order of characters
446
447         if( bPadRight )
448         {
449                 while(PadLength-- > 0)
450                         putch_cb(putch_h, PadChar), ret ++;
451         }
452         
453         return ret;
454 }
455
456 int expand_double(double num, uint64_t *Significand, int16_t *Exponent, int *SignIsNeg)
457 {
458         // IEEE 754 binary64
459         #if 0
460         {
461                 uint64_t test_exp = 0xC000000000000000;
462                 double test_value = -2.0f;
463                 assert( *((uint64_t*)&test_value) == test_exp );
464         }
465         #endif
466         
467         const uint64_t  *bit_rep = (void*)&num;
468         
469         *SignIsNeg = *bit_rep >> 63;
470         *Exponent = ((*bit_rep >> 52) & 0x7FF) - 1023;
471         *Significand = (*bit_rep & ((1ULL << 52)-1)) << (64-52);
472
473 //      printf("%llx %i %i %llx\n", *bit_rep, (int)*SignIsNeg, (int)*Exponent, *Significand);
474
475         // Subnormals
476         if( *Exponent == -1023 && *Significand != 0 )
477                 return 1;
478         // Infinity
479         if( *Exponent == 0x800 && *Significand == 0)
480                 return 2;
481         // NaNs
482         if( *Exponent == 0x800 && *Significand != 0)
483                 return 3;
484
485         return 0;
486 }
487
488 /**
489  * Internal function
490  * \return Remainder
491  */
492 double _longdiv(double num, double den, int *quot)
493 {
494         assert(num >= 0);
495         assert(den > 0);
496 //      printf("%llu / %llu\n", (long long int)num, (long long int)den);
497         
498         *quot = 0;
499         assert(num < den*10);
500         while(num >= den)
501         {
502                 num -= den;
503                 (*quot) ++;
504                 assert( *quot < 10 );
505         }
506 //      printf(" %i\n", *quot);
507         return num;
508 }
509
510 size_t _printf_ftoa_hex(printf_putch_t putch_cb, void *putch_h, long double num, int Precision, int bForcePoint, int bForceSign, int bCapitals)
511 {
512         uint64_t        significand;
513         int16_t exponent;
514          int    signisneg;
515          int    rv;
516         size_t  ret = 0;
517
518         #define _putch(_ch) do{\
519                 if(bCapitals)\
520                         putch_cb(putch_h, toupper(_ch));\
521                 else\
522                         putch_cb(putch_h, _ch);\
523                 ret ++;\
524         }while(0)
525
526         if( Precision == -1 )
527                 Precision = (64-4)/4;
528
529         rv = expand_double(num, &significand, &exponent, &signisneg);
530         switch(rv)
531         {
532         // 0: No errors, nothing special
533         case 0:
534                 break;
535         // 1: Subnormals
536         case 1:
537                 // TODO: Subnormal = 0?
538                 break;
539         // 2: Infinity
540         case 2:
541                 _putch('i');
542                 _putch('n');
543                 _putch('f');
544                 return 3;
545         case 3:
546                 _putch('N');
547                 _putch('a');
548                 _putch('N');
549                 return 3;
550         }
551         
552         // int  exp_ofs = exponent % 4;
553         // int  exp_16 = exponent - exp_ofs;
554         //uint8_t       whole = (1 << exp_ofs) | ((significand >> (64-3)) >> exp_ofs);
555         //significand <<= 3 - exp_ofs;
556         uint8_t whole = (rv != 1) ? 1 : 0;
557         
558         if( signisneg )
559                 _putch('-');
560         else if( bForceSign )
561                 _putch('+');
562         else {
563         }
564         _putch('0');
565         _putch('x');
566         _putch(cDIGITS[whole]);
567         if( significand || bForcePoint )
568                 _putch('.');
569         // Fractional
570         while( significand && Precision -- )
571         {
572                 uint8_t val = significand >> (64-4);
573                 _putch(cDIGITS[val]);
574                 significand <<= 4;
575         }
576         _putch('p');
577         //ret += _printf_itoa(putch_cb, putch_h, exp_16, 16, bCapitals, TRUE, '+', 0, 0, '\0', 0);
578         ret += _printf_itoa(putch_cb, putch_h, exponent, 10, bCapitals, TRUE, '+', 0, 0, '\0', 0);
579         
580         #undef _putch
581         return ret;
582 }
583
584 size_t _printf_ftoa(printf_putch_t putch_cb, void *putch_h, long double num, size_t Base, enum eFPN Notation, int Precision, int bForcePoint, int bForceSign, int bCapitals)
585 {
586         uint64_t        significand;
587         int16_t exponent;
588          int    signisneg;
589          int    rv;
590         size_t  ret = 0;
591
592         #define _putch(_ch) do{\
593                 if(bCapitals)\
594                         putch_cb(putch_h, toupper(_ch));\
595                 else\
596                         putch_cb(putch_h, _ch);\
597                 ret ++;\
598         }while(0)
599
600         if( Base <= 1 || Base > 16 )
601                 return 0;
602
603         rv = expand_double(num, &significand, &exponent, &signisneg);
604         switch(rv)
605         {
606         // 0: No errors, nothing special
607         case 0:
608                 break;
609         // 1: Subnormals
610         case 1:
611                 // TODO: Subnormal = 0?
612                 break;
613         // 2: Infinity
614         case 2:
615                 _putch('i');
616                 _putch('n');
617                 _putch('f');
618                 return 3;
619         case 3:
620                 _putch('N');
621                 _putch('a');
622                 _putch('N');
623                 return 3;
624         }
625
626         // - Used as 0/1 bools in arithmatic later on
627         bForcePoint = !!bForcePoint;
628         bForceSign = !!bForceSign;
629
630         // Apply default to precision
631         if( Precision == -1 )
632                 Precision = 6;
633
634         if( num < 0 )
635                 num = -num;
636
637         // Select optimum type
638         if( Notation == FPN_SHORTEST )
639         {
640                 //TODO:
641                 //int   first_set_sig = BSL(significand);
642                 // bSign+log10(num)+1+precision vs. bSign+1+1+precision+1+1+log10(exponent)
643                  int    log10_num = exponent * 301 / 1000;      // log_10(2) = 0.30102999566...
644                  int    log10_exp10 = 2;
645                  int    sci_len = (signisneg || bForceSign) + 2 + (Precision-1) + 2 + log10_exp10;
646                  int    std_whole_len = (log10_num > 0 ? log10_num : 1);
647                  int    std_len = (signisneg || bForceSign) + std_whole_len + 1 + (Precision-std_whole_len);
648                 if( sci_len > std_len ) {
649                         Precision -= std_whole_len;
650                         Notation = FPN_STD;
651                 }
652                 else {
653                         Precision -= 1;
654                         Notation = FPN_SCI;
655                 }
656         }
657
658         double precision_max = 1;
659         for(int i = Precision; i--; )
660                 precision_max /= Base;
661
662         // Determine scientific's exponent and starting denominator
663         double den = 1;
664          int    sci_exponent = 0;
665         if( Notation == FPN_SCI )
666         {
667                 if( num < 1 )
668                 {
669                         while(num < 1)
670                         {
671                                 num *= Base;
672                                 sci_exponent ++;
673                         }
674                 }
675                 else if( num >= Base )
676                 {
677                         while(num >= Base)
678                         {
679                                 num /= Base;
680                                 sci_exponent ++;
681                         }
682                 }
683                 else
684                 {
685                         // no exponent
686                 }
687                 den = 1;
688         }
689         else
690         {
691                 while( den <= num )
692                         den *= Base;
693                 den /= Base;
694         }
695
696         // Leading sign
697         if( signisneg )
698                 _putch('-');
699         else if( bForceSign )
700                 _putch('+');
701         else {
702         }
703         
704         num += precision_max/10 * 5;
705
706          int    value;
707         // Whole section
708         do
709         {
710                 num = _longdiv(num, den, &value);
711                 _putch(cDIGITS[value]);
712                 den /= Base;
713         } while( den >= 1 );
714
715         // Decimal point (if needed/forced)     
716         if( Precision > 0 || bForcePoint )
717                 _putch('.');
718         // Decimal section
719         for(int i = Precision; i--; )
720         {
721                 num = _longdiv(num, den, &value);
722                 _putch(cDIGITS[value]);
723                 den /= Base;
724         }
725
726         if( Notation == FPN_SCI )
727         {
728                 if( Base == 16 )
729                         _putch('p');
730                 else
731                         _putch('e');
732                 ret += _printf_itoa(putch_cb, putch_h, sci_exponent, Base, FALSE, TRUE, '+', 3, 0, '\0', FALSE);
733         }       
734
735         #undef _putch
736
737         return ret;
738 }

UCC git Repository :: git.ucc.asn.au