1 // SPDX-License-Identifier: LGPL-2.1+
2 /*
3 * Tiny printf version for SPL
4 *
5 * Copied from:
6 * http://www.sparetimelabs.com/printfrevisited/printfrevisited.php
7 *
8 * Copyright (C) 2004,2008 Kustaa Nyholm
9 */
10
11 #include <log.h>
12 #include <serial.h>
13 #include <stdarg.h>
14 #include <linux/ctype.h>
15
16 struct printf_info {
17 char *bf; /* Digit buffer */
18 char zs; /* non-zero if a digit has been written */
19 char *outstr; /* Next output position for sprintf() */
20
21 /* Output a character */
22 void (*putc)(struct printf_info *info, char ch);
23 };
24
out(struct printf_info * info,char c)25 static void out(struct printf_info *info, char c)
26 {
27 *info->bf++ = c;
28 }
29
out_dgt(struct printf_info * info,char dgt)30 static 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
div_out(struct printf_info * info,unsigned long * num,unsigned long div)36 static 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
string(struct printf_info * info,char * s)51 static void string(struct printf_info *info, char *s)
52 {
53 char ch;
54
55 while ((ch = *s++))
56 out(info, ch);
57 }
58
59 static 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
pack_hex_byte(char * buf,u8 byte)63 static 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
mac_address_string(struct printf_info * info,u8 * addr,bool separator)70 static void mac_address_string(struct printf_info *info, u8 *addr,
71 bool separator)
72 {
73 /* (6 * 2 hex digits), 5 colons and trailing zero */
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
put_dec_trunc(char * buf,unsigned int q)88 static 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'; /* least significant digit */
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'; /* next digit */
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'; /* next digit */
110
111 d3 = q + 4 * d3;
112 if (d3 != 0) {
113 q = (d3 * 0xcd) >> 11;
114 d3 = d3 - 10 * q;
115 *buf++ = d3 + '0'; /* next digit */
116 if (q != 0)
117 *buf++ = q + '0'; /* most sign. digit */
118 }
119 }
120 }
121 return buf;
122 }
123
ip4_addr_string(struct printf_info * info,u8 * addr)124 static void ip4_addr_string(struct printf_info *info, u8 *addr)
125 {
126 /* (4 * 3 decimal digits), 3 dots and trailing zero */
127 char ip4_addr[4 * 4];
128 char temp[3]; /* hold each IP quad in reverse order */
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 /* reverse the digits in the quad */
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 /* CONFIG_SPL_NET */
145
146 /*
147 * Show a '%p' thing. A kernel extension is that the '%p' is followed
148 * by an extra set of characters that are extended format
149 * specifiers.
150 *
151 * Right now we handle:
152 *
153 * - 'M' For a 6-byte MAC address, it prints the address in the
154 * usual colon-separated hex notation.
155 * - 'm' Same as above except there is no colon-separator.
156 * - 'I4'for IPv4 addresses printed in the usual way (dot-separated
157 * decimal).
158 */
159
160 #if defined(CONFIG_SPL_USE_TINY_PRINTF_POINTER_SUPPORT) || defined(DEBUG)
pointer(struct printf_info * info,const char * fmt,void * ptr)161 static void pointer(struct printf_info *info, const char *fmt, void *ptr)
162 {
163 unsigned long num = (uintptr_t)ptr;
164 unsigned long div;
165
166 switch (*fmt) {
167 case 'a':
168 switch (fmt[1]) {
169 case 'p':
170 default:
171 num = *(phys_addr_t *)ptr;
172 break;
173 }
174 break;
175 #ifdef CONFIG_SPL_NET
176 case 'm':
177 return mac_address_string(info, ptr, false);
178 case 'M':
179 return mac_address_string(info, ptr, true);
180 case 'I':
181 if (fmt[1] == '4')
182 return ip4_addr_string(info, ptr);
183 #else
184 case 'm':
185 case 'M':
186 case 'I':
187 out(info, '?');
188 return;
189 #endif
190 default:
191 break;
192 }
193
194 div = 1UL << (sizeof(long) * 8 - 4);
195 for (; div; div /= 0x10)
196 div_out(info, &num, div);
197 }
198 #endif
199
_vprintf(struct printf_info * info,const char * fmt,va_list va)200 static int _vprintf(struct printf_info *info, const char *fmt, va_list va)
201 {
202 char ch;
203 char *p;
204 unsigned long num;
205 char buf[12];
206 unsigned long div;
207
208 while ((ch = *(fmt++))) {
209 if (ch != '%') {
210 info->putc(info, ch);
211 } else {
212 bool lz = false;
213 int width = 0;
214 bool islong = false;
215 bool force_char = false;
216
217 ch = *(fmt++);
218 if (ch == '-')
219 ch = *(fmt++);
220
221 if (ch == '0') {
222 ch = *(fmt++);
223 lz = 1;
224 }
225
226 if (ch >= '0' && ch <= '9') {
227 width = 0;
228 while (ch >= '0' && ch <= '9') {
229 width = (width * 10) + ch - '0';
230 ch = *fmt++;
231 }
232 }
233 if (ch == 'l') {
234 ch = *(fmt++);
235 islong = true;
236 }
237
238 info->bf = buf;
239 p = info->bf;
240 info->zs = 0;
241
242 switch (ch) {
243 case '\0':
244 goto abort;
245 case 'u':
246 case 'd':
247 case 'i':
248 div = 1000000000;
249 if (islong) {
250 num = va_arg(va, unsigned long);
251 if (sizeof(long) > 4)
252 div *= div * 10;
253 } else {
254 num = va_arg(va, unsigned int);
255 }
256
257 if (ch != 'u') {
258 if (islong && (long)num < 0) {
259 num = -(long)num;
260 out(info, '-');
261 } else if (!islong && (int)num < 0) {
262 num = -(int)num;
263 out(info, '-');
264 }
265 }
266 if (!num) {
267 out_dgt(info, 0);
268 } else {
269 for (; div; div /= 10)
270 div_out(info, &num, div);
271 }
272 break;
273 #if defined(CONFIG_SPL_USE_TINY_PRINTF_POINTER_SUPPORT) || defined(DEBUG)
274 case 'p':
275 pointer(info, fmt, va_arg(va, void *));
276 /*
277 * Skip this because it pulls in _ctype which is
278 * 256 bytes, and we don't generally implement
279 * pointer anyway
280 */
281 while (isalnum(fmt[0]))
282 fmt++;
283 break;
284 #endif
285 case 'x':
286 case 'X':
287 if (islong) {
288 num = va_arg(va, unsigned long);
289 div = 1UL << (sizeof(long) * 8 - 4);
290 } else {
291 num = va_arg(va, unsigned int);
292 div = 0x10000000;
293 }
294 if (!num) {
295 out_dgt(info, 0);
296 } else {
297 for (; div; div /= 0x10)
298 div_out(info, &num, div);
299 }
300 break;
301 case 'c':
302 out(info, (char)(va_arg(va, int)));
303 /* For the case when it's \0 char */
304 force_char = true;
305 break;
306 case 's':
307 p = va_arg(va, char*);
308 break;
309 case '%':
310 out(info, '%');
311 break;
312 default:
313 out(info, '?');
314 break;
315 }
316
317 *info->bf = 0;
318 info->bf = p;
319 while (width > 0 && info->bf && *info->bf++)
320 width--;
321 while (width-- > 0)
322 info->putc(info, lz ? '0' : ' ');
323 if (p) {
324 while ((ch = *p++) || force_char) {
325 info->putc(info, ch);
326 force_char = false;
327 }
328 }
329 }
330 }
331
332 abort:
333 return 0;
334 }
335
336 #if CONFIG_IS_ENABLED(PRINTF)
putc_normal(struct printf_info * info,char ch)337 static void putc_normal(struct printf_info *info, char ch)
338 {
339 putc(ch);
340 }
341
vprintf(const char * fmt,va_list va)342 int vprintf(const char *fmt, va_list va)
343 {
344 struct printf_info info;
345
346 info.putc = putc_normal;
347 return _vprintf(&info, fmt, va);
348 }
349
printf(const char * fmt,...)350 int printf(const char *fmt, ...)
351 {
352 struct printf_info info;
353
354 va_list va;
355 int ret;
356
357 info.putc = putc_normal;
358 va_start(va, fmt);
359 ret = _vprintf(&info, fmt, va);
360 va_end(va);
361
362 return ret;
363 }
364 #endif
365
putc_outstr(struct printf_info * info,char ch)366 static void putc_outstr(struct printf_info *info, char ch)
367 {
368 *info->outstr++ = ch;
369 }
370
sprintf(char * buf,const char * fmt,...)371 int sprintf(char *buf, const char *fmt, ...)
372 {
373 struct printf_info info;
374 va_list va;
375
376 va_start(va, fmt);
377 info.outstr = buf;
378 info.putc = putc_outstr;
379 _vprintf(&info, fmt, va);
380 va_end(va);
381 *info.outstr = '\0';
382
383 return info.outstr - buf;
384 }
385
386 #if CONFIG_IS_ENABLED(LOG)
387 /* Note that size is ignored */
vsnprintf(char * buf,size_t size,const char * fmt,va_list va)388 int vsnprintf(char *buf, size_t size, const char *fmt, va_list va)
389 {
390 struct printf_info info;
391
392 info.outstr = buf;
393 info.putc = putc_outstr;
394 _vprintf(&info, fmt, va);
395 *info.outstr = '\0';
396
397 return info.outstr - buf;
398 }
399 #endif
400
401 /* Note that size is ignored */
snprintf(char * buf,size_t size,const char * fmt,...)402 int snprintf(char *buf, size_t size, const char *fmt, ...)
403 {
404 struct printf_info info;
405 va_list va;
406
407 va_start(va, fmt);
408 info.outstr = buf;
409 info.putc = putc_outstr;
410 _vprintf(&info, fmt, va);
411 va_end(va);
412 *info.outstr = '\0';
413
414 return info.outstr - buf;
415 }
416
print_grouped_ull(unsigned long long int_val,int digits)417 void print_grouped_ull(unsigned long long int_val, int digits)
418 {
419 /* Don't try to print the upper 32-bits */
420 printf("%ld ", (ulong)int_val);
421 }
422