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