1 /*
2 * Copyright (c) 2008-2014 Travis Geiselbrecht
3 *
4 * Use of this source code is governed by a MIT-style
5 * license that can be found in the LICENSE file or at
6 * https://opensource.org/licenses/MIT
7 */
8 #include <lk/debug.h>
9 #include <assert.h>
10 #include <limits.h>
11 #include <printf.h>
12 #include <stdarg.h>
13 #include <sys/types.h>
14 #include <stdio.h>
15 #include <string.h>
16 #include <platform/debug.h>
17
18 #if WITH_NO_FP
19 #define FLOAT_PRINTF 0
20 #else
21 #define FLOAT_PRINTF 1
22 #endif
23
sprintf(char * str,const char * fmt,...)24 int sprintf(char *str, const char *fmt, ...) {
25 int err;
26
27 va_list ap;
28 va_start(ap, fmt);
29 err = vsprintf(str, fmt, ap);
30 va_end(ap);
31
32 return err;
33 }
34
snprintf(char * str,size_t len,const char * fmt,...)35 int snprintf(char *str, size_t len, const char *fmt, ...) {
36 int err;
37
38 va_list ap;
39 va_start(ap, fmt);
40 err = vsnprintf(str, len, fmt, ap);
41 va_end(ap);
42
43 return err;
44 }
45
vsprintf(char * str,const char * fmt,va_list ap)46 int vsprintf(char *str, const char *fmt, va_list ap) {
47 return vsnprintf(str, INT_MAX, fmt, ap);
48 }
49
50 struct _output_args {
51 char *outstr;
52 size_t len;
53 size_t pos;
54 };
55
_vsnprintf_output(const char * str,size_t len,void * state)56 static int _vsnprintf_output(const char *str, size_t len, void *state) {
57 struct _output_args *args = state;
58
59 size_t count = 0;
60 while (count < len) {
61 if (args->pos < args->len) {
62 args->outstr[args->pos++] = *str;
63 }
64
65 str++;
66 count++;
67 }
68
69 return count;
70 }
71
vsnprintf(char * str,size_t len,const char * fmt,va_list ap)72 int vsnprintf(char *str, size_t len, const char *fmt, va_list ap) {
73 struct _output_args args;
74 int wlen;
75
76 args.outstr = str;
77 args.len = len;
78 args.pos = 0;
79
80 wlen = _printf_engine(&_vsnprintf_output, (void *)&args, fmt, ap);
81 if (args.pos >= len)
82 str[len-1] = '\0';
83 else
84 str[wlen] = '\0';
85 return wlen;
86 }
87
88 #define LONGFLAG 0x00000001
89 #define LONGLONGFLAG 0x00000002
90 #define HALFFLAG 0x00000004
91 #define HALFHALFFLAG 0x00000008
92 #define SIZETFLAG 0x00000010
93 #define INTMAXFLAG 0x00000020
94 #define PTRDIFFFLAG 0x00000040
95 #define ALTFLAG 0x00000080
96 #define CAPSFLAG 0x00000100
97 #define SHOWSIGNFLAG 0x00000200
98 #define SIGNEDFLAG 0x00000400
99 #define LEFTFORMATFLAG 0x00000800
100 #define LEADZEROFLAG 0x00001000
101 #define BLANKPOSFLAG 0x00002000
102
longlong_to_string(char * buf,unsigned long long n,size_t len,uint flag,char * signchar)103 __NO_INLINE static char *longlong_to_string(char *buf, unsigned long long n, size_t len, uint flag, char *signchar) {
104 size_t pos = len;
105 int negative = 0;
106
107 if ((flag & SIGNEDFLAG) && (long long)n < 0) {
108 negative = 1;
109 n = -n;
110 }
111
112 buf[--pos] = 0;
113
114 /* only do the math if the number is >= 10 */
115 while (n >= 10) {
116 int digit = n % 10;
117
118 n /= 10;
119
120 buf[--pos] = digit + '0';
121 }
122 buf[--pos] = n + '0';
123
124 if (negative)
125 *signchar = '-';
126 else if ((flag & SHOWSIGNFLAG))
127 *signchar = '+';
128 else if ((flag & BLANKPOSFLAG))
129 *signchar = ' ';
130 else
131 *signchar = '\0';
132
133 return &buf[pos];
134 }
135
136 static const char hextable[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
137 static const char hextable_caps[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
138
longlong_to_hexstring(char * buf,unsigned long long u,size_t len,uint flag)139 __NO_INLINE static char *longlong_to_hexstring(char *buf, unsigned long long u, size_t len, uint flag) {
140 size_t pos = len;
141 const char *table = (flag & CAPSFLAG) ? hextable_caps : hextable;
142
143 buf[--pos] = 0;
144 do {
145 unsigned int digit = u % 16;
146 u /= 16;
147
148 buf[--pos] = table[digit];
149 } while (u != 0);
150
151 return &buf[pos];
152 }
153
154 #if FLOAT_PRINTF
155 union double_int {
156 double d;
157 uint64_t i;
158 };
159
160 #define OUT(c) buf[pos++] = (c)
161 #define OUTSTR(str) do { for (size_t i = 0; (str)[i] != 0; i++) OUT((str)[i]); } while (0)
162
163 /* print up to a 4 digit exponent as string, with sign */
exponent_to_string(char * buf,int32_t exponent)164 __NO_INLINE static size_t exponent_to_string(char *buf, int32_t exponent) {
165 size_t pos = 0;
166
167 /* handle sign */
168 if (exponent < 0) {
169 OUT('-');
170 exponent = -exponent;
171 } else {
172 OUT('+');
173 }
174
175 /* see how far we need to bump into the string to print from the right */
176 if (exponent >= 1000) pos += 4;
177 else if (exponent >= 100) pos += 3;
178 else if (exponent >= 10) pos += 2;
179 else pos++;
180
181 /* print decimal string, from the right */
182 uint i = pos;
183 do {
184 uint digit = (uint32_t)exponent % 10;
185
186 buf[--i] = digit + '0';
187
188 exponent /= 10;
189 } while (exponent != 0);
190
191 /* return number of characters printed */
192 return pos;
193 }
194
double_to_string(char * buf,size_t len,double d,uint flag)195 __NO_INLINE static char *double_to_string(char *buf, size_t len, double d, uint flag) {
196 size_t pos = 0;
197 union double_int du = { d };
198
199 uint32_t exponent = (du.i >> 52) & 0x7ff;
200 uint64_t fraction = (du.i & ((1ULL << 52) - 1));
201 bool neg = !!(du.i & (1ULL << 63));
202
203 /* start constructing the string */
204 if (neg) {
205 OUT('-');
206 d = -d;
207 }
208
209 /* longest:
210 * 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000o
211 */
212
213 /* look for special cases */
214 if (exponent == 0x7ff) {
215 if (fraction == 0) {
216 /* infinity */
217 if (flag & CAPSFLAG) OUTSTR("INF");
218 else OUTSTR("inf");
219 } else {
220 /* NaN */
221 if (flag & CAPSFLAG) OUTSTR("NAN");
222 else OUTSTR("nan");
223 }
224 } else if (exponent == 0) {
225 if (fraction == 0) {
226 /* zero */
227 OUTSTR("0.000000");
228 } else {
229 /* denormalized */
230 /* XXX does not handle */
231 if (flag & CAPSFLAG) OUTSTR("DEN");
232 else OUTSTR("den");
233 }
234 } else {
235 /* see if it's in the range of floats we can easily print */
236 int exponent_signed = exponent - 1023;
237 if (exponent_signed < -52 || exponent_signed > 52) {
238 OUTSTR("<range>");
239 } else {
240 /* start by walking backwards through the string */
241 #define OUTREV(c) do { if (&buf[pos] == buf) goto done; else buf[--pos] = (c); } while (0)
242 pos = len;
243 OUTREV(0);
244
245 /* reserve space for the fractional component first */
246 for (int i = 0; i <= 6; i++)
247 OUTREV('0');
248 size_t decimal_spot = pos;
249
250 /* print the integer portion */
251 uint64_t u;
252 if (exponent_signed >= 0) {
253 u = fraction;
254 u |= (1ULL<<52);
255 u >>= (52 - exponent_signed);
256
257 char *s = longlong_to_string(buf, u, pos + 1, flag, &(char) {0});
258
259 pos = s - buf;
260 } else {
261 /* exponent is negative */
262 u = 0;
263 OUTREV('0');
264 }
265
266 buf[decimal_spot] = '.';
267
268 /* handle the fractional part */
269 uint32_t frac = ((d - u) * 1000000) + .5;
270
271 uint i = decimal_spot + 6 + 1;
272 while (frac != 0) {
273 uint digit = frac % 10;
274
275 buf[--i] = digit + '0';
276
277 frac /= 10;
278 }
279
280 if (neg)
281 OUTREV('-');
282
283 done:
284 /* separate return path, since we've been walking backwards through the string */
285 return &buf[pos];
286 }
287 #undef OUTREV
288 }
289
290 buf[pos] = 0;
291 return buf;
292 }
293
double_to_hexstring(char * buf,size_t len,double d,uint flag)294 __NO_INLINE static char *double_to_hexstring(char *buf, size_t len, double d, uint flag) {
295 size_t pos = 0;
296 union double_int u = { d };
297
298 uint32_t exponent = (u.i >> 52) & 0x7ff;
299 uint64_t fraction = (u.i & ((1ULL << 52) - 1));
300 bool neg = !!(u.i & (1ULL << 63));
301
302 /* start constructing the string */
303 if (neg) {
304 OUT('-');
305 }
306
307 /* look for special cases */
308 if (exponent == 0x7ff) {
309 if (fraction == 0) {
310 /* infinity */
311 if (flag & CAPSFLAG) OUTSTR("INF");
312 else OUTSTR("inf");
313 } else {
314 /* NaN */
315 if (flag & CAPSFLAG) OUTSTR("NAN");
316 else OUTSTR("nan");
317 }
318 } else if (exponent == 0) {
319 if (fraction == 0) {
320 /* zero */
321 if (flag & CAPSFLAG) OUTSTR("0X0P+0");
322 else OUTSTR("0x0p+0");
323 } else {
324 /* denormalized */
325 /* XXX does not handle */
326 if (flag & CAPSFLAG) OUTSTR("DEN");
327 else OUTSTR("den");
328 }
329 } else {
330 /* regular normalized numbers:
331 * 0x1p+1
332 * 0x1.0000000000001p+1
333 * 0X1.FFFFFFFFFFFFFP+1023
334 * 0x1.FFFFFFFFFFFFFP+1023
335 */
336 int exponent_signed = exponent - 1023;
337
338 /* implicit 1. */
339 if (flag & CAPSFLAG) OUTSTR("0X1");
340 else OUTSTR("0x1");
341
342 /* select the appropriate hex case table */
343 const char *table = (flag & CAPSFLAG) ? hextable_caps : hextable;
344
345 int zero_count = 0;
346 bool output_dot = false;
347 for (int i = 52 - 4; i >= 0; i -= 4) {
348 uint digit = (fraction >> i) & 0xf;
349
350 if (digit == 0) {
351 zero_count++;
352 } else {
353 /* output a . the first time we output a char */
354 if (!output_dot) {
355 OUT('.');
356 output_dot = true;
357 }
358 /* if we have a non zero digit, see if we need to output a string of zeros */
359 while (zero_count > 0) {
360 OUT('0');
361 zero_count--;
362 }
363 buf[pos++] = table[digit];
364 }
365 }
366
367 /* handle the exponent */
368 buf[pos++] = (flag & CAPSFLAG) ? 'P' : 'p';
369 pos += exponent_to_string(&buf[pos], exponent_signed);
370 }
371
372 buf[pos] = 0;
373 return buf;
374 }
375
376 #undef OUT
377 #undef OUTSTR
378
379 #endif // FLOAT_PRINTF
380
_printf_engine(_printf_engine_output_func out,void * state,const char * fmt,va_list ap)381 int _printf_engine(_printf_engine_output_func out, void *state, const char *fmt, va_list ap) {
382 int err = 0;
383 char c;
384 unsigned char uc;
385 const char *s;
386 size_t string_len;
387 unsigned long long n;
388 void *ptr;
389 int flags;
390 unsigned int format_num;
391 char signchar;
392 size_t chars_written = 0;
393 char num_buffer[32];
394
395 #define OUTPUT_STRING(str, len) do { err = out(str, len, state); if (err < 0) { goto exit; } else { chars_written += err; } } while(0)
396 #define OUTPUT_CHAR(c) do { char __temp[1] = { c }; OUTPUT_STRING(__temp, 1); } while (0)
397
398 for (;;) {
399 /* reset the format state */
400 flags = 0;
401 format_num = 0;
402 signchar = '\0';
403
404 /* handle regular chars that aren't format related */
405 s = fmt;
406 string_len = 0;
407 while ((c = *fmt++) != 0) {
408 if (c == '%')
409 break; /* we saw a '%', break and start parsing format */
410 string_len++;
411 }
412
413 /* output the string we've accumulated */
414 OUTPUT_STRING(s, string_len);
415
416 /* make sure we haven't just hit the end of the string */
417 if (c == 0)
418 break;
419
420 next_format:
421 /* grab the next format character */
422 c = *fmt++;
423 if (c == 0)
424 break;
425
426 switch (c) {
427 case '0'...'9':
428 if (c == '0' && format_num == 0)
429 flags |= LEADZEROFLAG;
430 format_num *= 10;
431 format_num += c - '0';
432 goto next_format;
433 case '.':
434 /* XXX for now eat numeric formatting */
435 goto next_format;
436 case '%':
437 OUTPUT_CHAR('%');
438 break;
439 case 'c':
440 uc = va_arg(ap, unsigned int);
441 OUTPUT_CHAR(uc);
442 break;
443 case 's':
444 s = va_arg(ap, const char *);
445 if (s == 0)
446 s = "<null>";
447 flags &= ~LEADZEROFLAG; /* doesn't make sense for strings */
448 goto _output_string;
449 case '-':
450 flags |= LEFTFORMATFLAG;
451 goto next_format;
452 case '+':
453 flags |= SHOWSIGNFLAG;
454 goto next_format;
455 case ' ':
456 flags |= BLANKPOSFLAG;
457 goto next_format;
458 case '#':
459 flags |= ALTFLAG;
460 goto next_format;
461 case 'l':
462 if (flags & LONGFLAG)
463 flags |= LONGLONGFLAG;
464 flags |= LONGFLAG;
465 goto next_format;
466 case 'h':
467 if (flags & HALFFLAG)
468 flags |= HALFHALFFLAG;
469 flags |= HALFFLAG;
470 goto next_format;
471 case 'z':
472 flags |= SIZETFLAG;
473 goto next_format;
474 case 'j':
475 flags |= INTMAXFLAG;
476 goto next_format;
477 case 't':
478 flags |= PTRDIFFFLAG;
479 goto next_format;
480 case 'i':
481 case 'd':
482 n = (flags & LONGLONGFLAG) ? va_arg(ap, long long) :
483 (flags & LONGFLAG) ? va_arg(ap, long) :
484 (flags & HALFHALFFLAG) ? (signed char)va_arg(ap, int) :
485 (flags & HALFFLAG) ? (short)va_arg(ap, int) :
486 (flags & SIZETFLAG) ? va_arg(ap, ssize_t) :
487 (flags & INTMAXFLAG) ? va_arg(ap, intmax_t) :
488 (flags & PTRDIFFFLAG) ? va_arg(ap, ptrdiff_t) :
489 va_arg(ap, int);
490 flags |= SIGNEDFLAG;
491 s = longlong_to_string(num_buffer, n, sizeof(num_buffer), flags, &signchar);
492 goto _output_string;
493 case 'u':
494 n = (flags & LONGLONGFLAG) ? va_arg(ap, unsigned long long) :
495 (flags & LONGFLAG) ? va_arg(ap, unsigned long) :
496 (flags & HALFHALFFLAG) ? (unsigned char)va_arg(ap, unsigned int) :
497 (flags & HALFFLAG) ? (unsigned short)va_arg(ap, unsigned int) :
498 (flags & SIZETFLAG) ? va_arg(ap, size_t) :
499 (flags & INTMAXFLAG) ? va_arg(ap, uintmax_t) :
500 (flags & PTRDIFFFLAG) ? (uintptr_t)va_arg(ap, ptrdiff_t) :
501 va_arg(ap, unsigned int);
502 s = longlong_to_string(num_buffer, n, sizeof(num_buffer), flags, &signchar);
503 goto _output_string;
504 case 'p':
505 flags |= LONGFLAG | ALTFLAG;
506 goto hex;
507 case 'X':
508 flags |= CAPSFLAG;
509 /* fallthrough */
510 hex:
511 case 'x':
512 n = (flags & LONGLONGFLAG) ? va_arg(ap, unsigned long long) :
513 (flags & LONGFLAG) ? va_arg(ap, unsigned long) :
514 (flags & HALFHALFFLAG) ? (unsigned char)va_arg(ap, unsigned int) :
515 (flags & HALFFLAG) ? (unsigned short)va_arg(ap, unsigned int) :
516 (flags & SIZETFLAG) ? va_arg(ap, size_t) :
517 (flags & INTMAXFLAG) ? va_arg(ap, uintmax_t) :
518 (flags & PTRDIFFFLAG) ? (uintptr_t)va_arg(ap, ptrdiff_t) :
519 va_arg(ap, unsigned int);
520 s = longlong_to_hexstring(num_buffer, n, sizeof(num_buffer), flags);
521 if (flags & ALTFLAG) {
522 OUTPUT_CHAR('0');
523 OUTPUT_CHAR((flags & CAPSFLAG) ? 'X': 'x');
524 }
525 goto _output_string;
526 case 'n':
527 ptr = va_arg(ap, void *);
528 if (flags & LONGLONGFLAG)
529 *(long long *)ptr = chars_written;
530 else if (flags & LONGFLAG)
531 *(long *)ptr = chars_written;
532 else if (flags & HALFHALFFLAG)
533 *(signed char *)ptr = chars_written;
534 else if (flags & HALFFLAG)
535 *(short *)ptr = chars_written;
536 else if (flags & SIZETFLAG)
537 *(size_t *)ptr = chars_written;
538 else
539 *(int *)ptr = chars_written;
540 break;
541 #if FLOAT_PRINTF
542 case 'F':
543 flags |= CAPSFLAG;
544 /* fallthrough */
545 case 'f': {
546 double d = va_arg(ap, double);
547 s = double_to_string(num_buffer, sizeof(num_buffer), d, flags);
548 goto _output_string;
549 }
550 case 'A':
551 flags |= CAPSFLAG;
552 /* fallthrough */
553 case 'a': {
554 double d = va_arg(ap, double);
555 s = double_to_hexstring(num_buffer, sizeof(num_buffer), d, flags);
556 goto _output_string;
557 }
558 #endif
559 default:
560 OUTPUT_CHAR('%');
561 OUTPUT_CHAR(c);
562 break;
563 }
564
565 /* move on to the next field */
566 continue;
567
568 /* shared output code */
569 _output_string:
570 string_len = strlen(s);
571
572 if (flags & LEFTFORMATFLAG) {
573 /* left justify the text */
574 OUTPUT_STRING(s, string_len);
575 uint written = err;
576
577 /* pad to the right (if necessary) */
578 for (; format_num > written; format_num--)
579 OUTPUT_CHAR(' ');
580 } else {
581 /* right justify the text (digits) */
582
583 /* if we're going to print a sign digit,
584 it'll chew up one byte of the format size */
585 if (signchar != '\0' && format_num > 0)
586 format_num--;
587
588 /* output the sign char before the leading zeros */
589 if (flags & LEADZEROFLAG && signchar != '\0')
590 OUTPUT_CHAR(signchar);
591
592 /* pad according to the format string */
593 for (; format_num > string_len; format_num--)
594 OUTPUT_CHAR(flags & LEADZEROFLAG ? '0' : ' ');
595
596 /* if not leading zeros, output the sign char just before the number */
597 if (!(flags & LEADZEROFLAG) && signchar != '\0')
598 OUTPUT_CHAR(signchar);
599
600 /* output the string */
601 OUTPUT_STRING(s, string_len);
602 }
603 continue;
604 }
605
606 #undef OUTPUT_STRING
607 #undef OUTPUT_CHAR
608
609 exit:
610 return (err < 0) ? err : (int)chars_written;
611 }
612