1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
#include <stdarg.h>
#include <string.h>
#include <stdint.h>
#include <BoardConsole.h>
#include <osdebug.h>
static const char hex_conv[] = "0123456789ABCDEF";
static void dbgput(const void * _str, int n) {
const char * str = (const char *) _str;
while(n--)
BoardConsolePutc(*(str++));
}
void osDbgPrintf(const char * str, ...) {
if (!str)
return;
va_list ap;
const char * percent;
const char * ptr = str;
int entry_size;
char tmp_n_conv[33], * tmp_conv_ptr;
int seen_something;
int arg_c;
char * arg_s;
unsigned long arg_u = 0;
long arg_i;
uintptr_t arg_p;
int str_size, i;
va_start(ap, str);
while ((percent = strchr(ptr, '%'))) {
dbgput(ptr, percent - ptr);
ptr = percent + 1;
entry_size = 0;
while ((*ptr >= '0') && (*ptr <= '9')) {
entry_size *= 10;
entry_size += *ptr - '0';
ptr++;
}
switch (*ptr) {
case '%':
dbgput("%", 1);
break;
case 'c':
arg_c = va_arg(ap, int);
dbgput(&arg_c, 1);
break;
case 's':
arg_s = va_arg(ap, char *);
if (arg_s) {
for (str_size = strlen(arg_s); str_size < entry_size; str_size++)
dbgput(" ", 1);
dbgput(arg_s, strlen(arg_s));
} else {
for (str_size = 0; str_size < entry_size; str_size++)
dbgput(" ", 1);
}
break;
case 'i':
case 'd':
arg_i = va_arg(ap, long);
if (arg_i < 0) {
dbgput("-", 1);
arg_u = -arg_i;
} else {
arg_u = arg_i;
}
case 'u':
if (*ptr == 'u')
arg_u = va_arg(ap, unsigned long);
tmp_conv_ptr = tmp_n_conv + 32;
*tmp_conv_ptr = 0;
do {
*--tmp_conv_ptr = hex_conv[arg_u % 10];
arg_u /= 10;
} while (arg_u);
dbgput(tmp_conv_ptr, strlen(tmp_conv_ptr));
break;
case 'p':
arg_p = va_arg(ap, uintptr_t);
dbgput("0x", 2);
for (i = sizeof(arg_p) * 2 - 1; i >= 0; i--) {
dbgput(&hex_conv[(arg_p >> (i << 2)) & 15], 1);
}
break;
case 'x':
arg_u = va_arg(ap, unsigned long);
seen_something = 0;
for (i = sizeof(arg_p) * 2 - 1; i >= 0; i--) {
if (!seen_something && ((arg_u >> (i << 2)) == 0))
continue;
dbgput(&hex_conv[(arg_u >> (i << 2)) & 15], 1);
seen_something = 1;
}
if (!seen_something)
dbgput("0", 1);
break;
case 0: // malformed format string with a trailing %.
dbgput("%", 1);
return;
default:
dbgput("<unsupported format: %", 22);
dbgput(ptr, 1);
dbgput(">", 1);
break;
}
ptr++;
}
if (*ptr)
dbgput(ptr, strlen(ptr));
}
|
