1
2
3
4
5
6
7
8
9
10
11
12#include <common.h>
13#include <stdarg.h>
14#include <serial.h>
15
16struct printf_info {
17 char *bf;
18 char zs;
19 char *outstr;
20
21
22 void (*putc)(struct printf_info *info, char ch);
23};
24
25void putc_normal(struct printf_info *info, char ch)
26{
27 putc(ch);
28}
29
30static void out(struct printf_info *info, char c)
31{
32 *info->bf++ = c;
33}
34
35static void out_dgt(struct printf_info *info, char dgt)
36{
37 out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
38 info->zs = 1;
39}
40
41static void div_out(struct printf_info *info, unsigned int *num,
42 unsigned int div)
43{
44 unsigned char dgt = 0;
45
46 while (*num >= div) {
47 *num -= div;
48 dgt++;
49 }
50
51 if (info->zs || dgt > 0)
52 out_dgt(info, dgt);
53}
54
55int _vprintf(struct printf_info *info, const char *fmt, va_list va)
56{
57 char ch;
58 char *p;
59 unsigned int num;
60 char buf[12];
61 unsigned int div;
62
63 while ((ch = *(fmt++))) {
64 if (ch != '%') {
65 info->putc(info, ch);
66 } else {
67 bool lz = false;
68 int width = 0;
69
70 ch = *(fmt++);
71 if (ch == '0') {
72 ch = *(fmt++);
73 lz = 1;
74 }
75
76 if (ch >= '0' && ch <= '9') {
77 width = 0;
78 while (ch >= '0' && ch <= '9') {
79 width = (width * 10) + ch - '0';
80 ch = *fmt++;
81 }
82 }
83 info->bf = buf;
84 p = info->bf;
85 info->zs = 0;
86
87 switch (ch) {
88 case '\0':
89 goto abort;
90 case 'u':
91 case 'd':
92 num = va_arg(va, unsigned int);
93 if (ch == 'd' && (int)num < 0) {
94 num = -(int)num;
95 out(info, '-');
96 }
97 if (!num) {
98 out_dgt(info, 0);
99 } else {
100 for (div = 1000000000; div; div /= 10)
101 div_out(info, &num, div);
102 }
103 break;
104 case 'x':
105 num = va_arg(va, unsigned int);
106 if (!num) {
107 out_dgt(info, 0);
108 } else {
109 for (div = 0x10000000; div; div /= 0x10)
110 div_out(info, &num, div);
111 }
112 break;
113 case 'c':
114 out(info, (char)(va_arg(va, int)));
115 break;
116 case 's':
117 p = va_arg(va, char*);
118 break;
119 case '%':
120 out(info, '%');
121 default:
122 break;
123 }
124
125 *info->bf = 0;
126 info->bf = p;
127 while (*info->bf++ && width > 0)
128 width--;
129 while (width-- > 0)
130 info->putc(info, lz ? '0' : ' ');
131 if (p) {
132 while ((ch = *p++))
133 info->putc(info, ch);
134 }
135 }
136 }
137
138abort:
139 return 0;
140}
141
142int vprintf(const char *fmt, va_list va)
143{
144 struct printf_info info;
145
146 info.putc = putc_normal;
147 return _vprintf(&info, fmt, va);
148}
149
150int printf(const char *fmt, ...)
151{
152 struct printf_info info;
153
154 va_list va;
155 int ret;
156
157 info.putc = putc_normal;
158 va_start(va, fmt);
159 ret = _vprintf(&info, fmt, va);
160 va_end(va);
161
162 return ret;
163}
164
165static void putc_outstr(struct printf_info *info, char ch)
166{
167 *info->outstr++ = ch;
168}
169
170int sprintf(char *buf, const char *fmt, ...)
171{
172 struct printf_info info;
173 va_list va;
174 int ret;
175
176 va_start(va, fmt);
177 info.outstr = buf;
178 info.putc = putc_outstr;
179 ret = _vprintf(&info, fmt, va);
180 va_end(va);
181 *info.outstr = '\0';
182
183 return ret;
184}
185
186
187int snprintf(char *buf, size_t size, const char *fmt, ...)
188{
189 struct printf_info info;
190 va_list va;
191 int ret;
192
193 va_start(va, fmt);
194 info.outstr = buf;
195 info.putc = putc_outstr;
196 ret = _vprintf(&info, fmt, va);
197 va_end(va);
198 *info.outstr = '\0';
199
200 return ret;
201}
202
203void __assert_fail(const char *assertion, const char *file, unsigned line,
204 const char *function)
205{
206
207 printf("%s:%u: %s: Assertion `%s' failed.", file, line, function,
208 assertion);
209 hang();
210}
211
