1
2
3
4
5
6
7
8
9
10
11#include <common.h>
12#include <stdarg.h>
13#include <serial.h>
14#include <linux/ctype.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
25static void out(struct printf_info *info, char c)
26{
27 *info->bf++ = c;
28}
29
30static void out_dgt(struct printf_info *info, char dgt)
31{
32 out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
33 info->zs = 1;
34}
35
36static void div_out(struct printf_info *info, unsigned long *num,
37 unsigned long div)
38{
39 unsigned char dgt = 0;
40
41 while (*num >= div) {
42 *num -= div;
43 dgt++;
44 }
45
46 if (info->zs || dgt > 0)
47 out_dgt(info, dgt);
48}
49
50#ifdef CONFIG_SPL_NET_SUPPORT
51static void string(struct printf_info *info, char *s)
52{
53 char ch;
54
55 while ((ch = *s++))
56 out(info, ch);
57}
58
59static const char hex_asc[] = "0123456789abcdef";
60#define hex_asc_lo(x) hex_asc[((x) & 0x0f)]
61#define hex_asc_hi(x) hex_asc[((x) & 0xf0) >> 4]
62
63static inline char *pack_hex_byte(char *buf, u8 byte)
64{
65 *buf++ = hex_asc_hi(byte);
66 *buf++ = hex_asc_lo(byte);
67 return buf;
68}
69
70static void mac_address_string(struct printf_info *info, u8 *addr,
71 bool separator)
72{
73
74 char mac_addr[6 * 3];
75 char *p = mac_addr;
76 int i;
77
78 for (i = 0; i < 6; i++) {
79 p = pack_hex_byte(p, addr[i]);
80 if (separator && i != 5)
81 *p++ = ':';
82 }
83 *p = '\0';
84
85 string(info, mac_addr);
86}
87
88static char *put_dec_trunc(char *buf, unsigned int q)
89{
90 unsigned int d3, d2, d1, d0;
91 d1 = (q >> 4) & 0xf;
92 d2 = (q >> 8) & 0xf;
93 d3 = (q >> 12);
94
95 d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
96 q = (d0 * 0xcd) >> 11;
97 d0 = d0 - 10 * q;
98 *buf++ = d0 + '0';
99 d1 = q + 9 * d3 + 5 * d2 + d1;
100 if (d1 != 0) {
101 q = (d1 * 0xcd) >> 11;
102 d1 = d1 - 10 * q;
103 *buf++ = d1 + '0';
104
105 d2 = q + 2 * d2;
106 if ((d2 != 0) || (d3 != 0)) {
107 q = (d2 * 0xd) >> 7;
108 d2 = d2 - 10 * q;
109 *buf++ = d2 + '0';
110
111 d3 = q + 4 * d3;
112 if (d3 != 0) {
113 q = (d3 * 0xcd) >> 11;
114 d3 = d3 - 10 * q;
115 *buf++ = d3 + '0';
116 if (q != 0)
117 *buf++ = q + '0';
118 }
119 }
120 }
121 return buf;
122}
123
124static void ip4_addr_string(struct printf_info *info, u8 *addr)
125{
126
127 char ip4_addr[4 * 4];
128 char temp[3];
129 char *p = ip4_addr;
130 int i, digits;
131
132 for (i = 0; i < 4; i++) {
133 digits = put_dec_trunc(temp, addr[i]) - temp;
134
135 while (digits--)
136 *p++ = temp[digits];
137 if (i != 3)
138 *p++ = '.';
139 }
140 *p = '\0';
141
142 string(info, ip4_addr);
143}
144#endif
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160static void pointer(struct printf_info *info, const char *fmt, void *ptr)
161{
162#ifdef DEBUG
163 unsigned long num = (uintptr_t)ptr;
164 unsigned long div;
165#endif
166
167 switch (*fmt) {
168#ifdef DEBUG
169 case 'a':
170
171 switch (fmt[1]) {
172 case 'p':
173 default:
174 num = *(phys_addr_t *)ptr;
175 break;
176 }
177 break;
178#endif
179#ifdef CONFIG_SPL_NET_SUPPORT
180 case 'm':
181 return mac_address_string(info, ptr, false);
182 case 'M':
183 return mac_address_string(info, ptr, true);
184 case 'I':
185 if (fmt[1] == '4')
186 return ip4_addr_string(info, ptr);
187#endif
188 default:
189 break;
190 }
191#ifdef DEBUG
192 div = 1UL << (sizeof(long) * 8 - 4);
193 for (; div; div /= 0x10)
194 div_out(info, &num, div);
195#endif
196}
197
198static int _vprintf(struct printf_info *info, const char *fmt, va_list va)
199{
200 char ch;
201 char *p;
202 unsigned long num;
203 char buf[12];
204 unsigned long div;
205
206 while ((ch = *(fmt++))) {
207 if (ch != '%') {
208 info->putc(info, ch);
209 } else {
210 bool lz = false;
211 int width = 0;
212 bool islong = false;
213
214 ch = *(fmt++);
215 if (ch == '-')
216 ch = *(fmt++);
217
218 if (ch == '0') {
219 ch = *(fmt++);
220 lz = 1;
221 }
222
223 if (ch >= '0' && ch <= '9') {
224 width = 0;
225 while (ch >= '0' && ch <= '9') {
226 width = (width * 10) + ch - '0';
227 ch = *fmt++;
228 }
229 }
230 if (ch == 'l') {
231 ch = *(fmt++);
232 islong = true;
233 }
234
235 info->bf = buf;
236 p = info->bf;
237 info->zs = 0;
238
239 switch (ch) {
240 case '\0':
241 goto abort;
242 case 'u':
243 case 'd':
244 div = 1000000000;
245 if (islong) {
246 num = va_arg(va, unsigned long);
247 if (sizeof(long) > 4)
248 div *= div * 10;
249 } else {
250 num = va_arg(va, unsigned int);
251 }
252
253 if (ch == 'd') {
254 if (islong && (long)num < 0) {
255 num = -(long)num;
256 out(info, '-');
257 } else if (!islong && (int)num < 0) {
258 num = -(int)num;
259 out(info, '-');
260 }
261 }
262 if (!num) {
263 out_dgt(info, 0);
264 } else {
265 for (; div; div /= 10)
266 div_out(info, &num, div);
267 }
268 break;
269 case 'x':
270 if (islong) {
271 num = va_arg(va, unsigned long);
272 div = 1UL << (sizeof(long) * 8 - 4);
273 } else {
274 num = va_arg(va, unsigned int);
275 div = 0x10000000;
276 }
277 if (!num) {
278 out_dgt(info, 0);
279 } else {
280 for (; div; div /= 0x10)
281 div_out(info, &num, div);
282 }
283 break;
284 case 'c':
285 out(info, (char)(va_arg(va, int)));
286 break;
287 case 's':
288 p = va_arg(va, char*);
289 break;
290 case 'p':
291 pointer(info, fmt, va_arg(va, void *));
292 while (isalnum(fmt[0]))
293 fmt++;
294 break;
295 case '%':
296 out(info, '%');
297 default:
298 break;
299 }
300
301 *info->bf = 0;
302 info->bf = p;
303 while (*info->bf++ && width > 0)
304 width--;
305 while (width-- > 0)
306 info->putc(info, lz ? '0' : ' ');
307 if (p) {
308 while ((ch = *p++))
309 info->putc(info, ch);
310 }
311 }
312 }
313
314abort:
315 return 0;
316}
317
318#if CONFIG_IS_ENABLED(PRINTF)
319static void putc_normal(struct printf_info *info, char ch)
320{
321 putc(ch);
322}
323
324int vprintf(const char *fmt, va_list va)
325{
326 struct printf_info info;
327
328 info.putc = putc_normal;
329 return _vprintf(&info, fmt, va);
330}
331
332int printf(const char *fmt, ...)
333{
334 struct printf_info info;
335
336 va_list va;
337 int ret;
338
339 info.putc = putc_normal;
340 va_start(va, fmt);
341 ret = _vprintf(&info, fmt, va);
342 va_end(va);
343
344 return ret;
345}
346#endif
347
348static void putc_outstr(struct printf_info *info, char ch)
349{
350 *info->outstr++ = ch;
351}
352
353int sprintf(char *buf, const char *fmt, ...)
354{
355 struct printf_info info;
356 va_list va;
357 int ret;
358
359 va_start(va, fmt);
360 info.outstr = buf;
361 info.putc = putc_outstr;
362 ret = _vprintf(&info, fmt, va);
363 va_end(va);
364 *info.outstr = '\0';
365
366 return ret;
367}
368
369
370int snprintf(char *buf, size_t size, const char *fmt, ...)
371{
372 struct printf_info info;
373 va_list va;
374 int ret;
375
376 va_start(va, fmt);
377 info.outstr = buf;
378 info.putc = putc_outstr;
379 ret = _vprintf(&info, fmt, va);
380 va_end(va);
381 *info.outstr = '\0';
382
383 return ret;
384}
385