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 <common.h>
12 #include <log.h>
13 #include <serial.h>
14 #include <stdarg.h>
15 #include <linux/ctype.h>
16
17 struct printf_info {
18 char *bf; /* Digit buffer */
19 char zs; /* non-zero if a digit has been written */
20 char *outstr; /* Next output position for sprintf() */
21
22 /* Output a character */
23 void (*putc)(struct printf_info *info, char ch);
24 };
25
out(struct printf_info * info,char c)26 static void out(struct printf_info *info, char c)
27 {
28 *info->bf++ = c;
29 }
30
out_dgt(struct printf_info * info,char dgt)31 static void out_dgt(struct printf_info *info, char dgt)
32 {
33 out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
34 info->zs = 1;
35 }
36
div_out(struct printf_info * info,unsigned long * num,unsigned long div)37 static void div_out(struct printf_info *info, unsigned long *num,
38 unsigned long div)
39 {
40 unsigned char dgt = 0;
41
42 while (*num >= div) {
43 *num -= div;
44 dgt++;
45 }
46
47 if (info->zs || dgt > 0)
48 out_dgt(info, dgt);
49 }
50
51 #ifdef CONFIG_SPL_NET
string(struct printf_info * info,char * s)52 static void string(struct printf_info *info, char *s)
53 {
54 char ch;
55
56 while ((ch = *s++))
57 out(info, ch);
58 }
59
60 static const char hex_asc[] = "0123456789abcdef";
61 #define hex_asc_lo(x) hex_asc[((x) & 0x0f)]
62 #define hex_asc_hi(x) hex_asc[((x) & 0xf0) >> 4]
63
pack_hex_byte(char * buf,u8 byte)64 static inline char *pack_hex_byte(char *buf, u8 byte)
65 {
66 *buf++ = hex_asc_hi(byte);
67 *buf++ = hex_asc_lo(byte);
68 return buf;
69 }
70
mac_address_string(struct printf_info * info,u8 * addr,bool separator)71 static void mac_address_string(struct printf_info *info, u8 *addr,
72 bool separator)
73 {
74 /* (6 * 2 hex digits), 5 colons and trailing zero */
75 char mac_addr[6 * 3];
76 char *p = mac_addr;
77 int i;
78
79 for (i = 0; i < 6; i++) {
80 p = pack_hex_byte(p, addr[i]);
81 if (separator && i != 5)
82 *p++ = ':';
83 }
84 *p = '\0';
85
86 string(info, mac_addr);
87 }
88
put_dec_trunc(char * buf,unsigned int q)89 static char *put_dec_trunc(char *buf, unsigned int q)
90 {
91 unsigned int d3, d2, d1, d0;
92 d1 = (q >> 4) & 0xf;
93 d2 = (q >> 8) & 0xf;
94 d3 = (q >> 12);
95
96 d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
97 q = (d0 * 0xcd) >> 11;
98 d0 = d0 - 10 * q;
99 *buf++ = d0 + '0'; /* least significant digit */
100 d1 = q + 9 * d3 + 5 * d2 + d1;
101 if (d1 != 0) {
102 q = (d1 * 0xcd) >> 11;
103 d1 = d1 - 10 * q;
104 *buf++ = d1 + '0'; /* next digit */
105
106 d2 = q + 2 * d2;
107 if ((d2 != 0) || (d3 != 0)) {
108 q = (d2 * 0xd) >> 7;
109 d2 = d2 - 10 * q;
110 *buf++ = d2 + '0'; /* next digit */
111
112 d3 = q + 4 * d3;
113 if (d3 != 0) {
114 q = (d3 * 0xcd) >> 11;
115 d3 = d3 - 10 * q;
116 *buf++ = d3 + '0'; /* next digit */
117 if (q != 0)
118 *buf++ = q + '0'; /* most sign. digit */
119 }
120 }
121 }
122 return buf;
123 }
124
ip4_addr_string(struct printf_info * info,u8 * addr)125 static void ip4_addr_string(struct printf_info *info, u8 *addr)
126 {
127 /* (4 * 3 decimal digits), 3 dots and trailing zero */
128 char ip4_addr[4 * 4];
129 char temp[3]; /* hold each IP quad in reverse order */
130 char *p = ip4_addr;
131 int i, digits;
132
133 for (i = 0; i < 4; i++) {
134 digits = put_dec_trunc(temp, addr[i]) - temp;
135 /* reverse the digits in the quad */
136 while (digits--)
137 *p++ = temp[digits];
138 if (i != 3)
139 *p++ = '.';
140 }
141 *p = '\0';
142
143 string(info, ip4_addr);
144 }
145 #endif
146
147 /*
148 * Show a '%p' thing. A kernel extension is that the '%p' is followed
149 * by an extra set of characters that are extended format
150 * specifiers.
151 *
152 * Right now we handle:
153 *
154 * - 'M' For a 6-byte MAC address, it prints the address in the
155 * usual colon-separated hex notation.
156 * - 'm' Same as above except there is no colon-separator.
157 * - 'I4'for IPv4 addresses printed in the usual way (dot-separated
158 * decimal).
159 */
160
pointer(struct printf_info * info,const char * fmt,void * ptr)161 static void __maybe_unused pointer(struct printf_info *info, const char *fmt,
162 void *ptr)
163 {
164 #ifdef DEBUG
165 unsigned long num = (uintptr_t)ptr;
166 unsigned long div;
167 #endif
168
169 switch (*fmt) {
170 #ifdef DEBUG
171 case 'a':
172
173 switch (fmt[1]) {
174 case 'p':
175 default:
176 num = *(phys_addr_t *)ptr;
177 break;
178 }
179 break;
180 #endif
181 #ifdef CONFIG_SPL_NET
182 case 'm':
183 return mac_address_string(info, ptr, false);
184 case 'M':
185 return mac_address_string(info, ptr, true);
186 case 'I':
187 if (fmt[1] == '4')
188 return ip4_addr_string(info, ptr);
189 #endif
190 default:
191 break;
192 }
193 #ifdef DEBUG
194 div = 1UL << (sizeof(long) * 8 - 4);
195 for (; div; div /= 0x10)
196 div_out(info, &num, div);
197 #endif
198 }
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
216 ch = *(fmt++);
217 if (ch == '-')
218 ch = *(fmt++);
219
220 if (ch == '0') {
221 ch = *(fmt++);
222 lz = 1;
223 }
224
225 if (ch >= '0' && ch <= '9') {
226 width = 0;
227 while (ch >= '0' && ch <= '9') {
228 width = (width * 10) + ch - '0';
229 ch = *fmt++;
230 }
231 }
232 if (ch == 'l') {
233 ch = *(fmt++);
234 islong = true;
235 }
236
237 info->bf = buf;
238 p = info->bf;
239 info->zs = 0;
240
241 switch (ch) {
242 case '\0':
243 goto abort;
244 case 'u':
245 case 'd':
246 case 'i':
247 div = 1000000000;
248 if (islong) {
249 num = va_arg(va, unsigned long);
250 if (sizeof(long) > 4)
251 div *= div * 10;
252 } else {
253 num = va_arg(va, unsigned int);
254 }
255
256 if (ch != 'u') {
257 if (islong && (long)num < 0) {
258 num = -(long)num;
259 out(info, '-');
260 } else if (!islong && (int)num < 0) {
261 num = -(int)num;
262 out(info, '-');
263 }
264 }
265 if (!num) {
266 out_dgt(info, 0);
267 } else {
268 for (; div; div /= 10)
269 div_out(info, &num, div);
270 }
271 break;
272 case 'p':
273 if (CONFIG_IS_ENABLED(NET) || _DEBUG) {
274 pointer(info, fmt, va_arg(va, void *));
275 /*
276 * Skip this because it pulls in _ctype which is
277 * 256 bytes, and we don't generally implement
278 * pointer anyway
279 */
280 while (isalnum(fmt[0]))
281 fmt++;
282 break;
283 }
284 islong = true;
285 /* no break */
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 break;
304 case 's':
305 p = va_arg(va, char*);
306 break;
307 case '%':
308 out(info, '%');
309 default:
310 break;
311 }
312
313 *info->bf = 0;
314 info->bf = p;
315 while (*info->bf++ && width > 0)
316 width--;
317 while (width-- > 0)
318 info->putc(info, lz ? '0' : ' ');
319 if (p) {
320 while ((ch = *p++))
321 info->putc(info, ch);
322 }
323 }
324 }
325
326 abort:
327 return 0;
328 }
329
330 #if CONFIG_IS_ENABLED(PRINTF)
putc_normal(struct printf_info * info,char ch)331 static void putc_normal(struct printf_info *info, char ch)
332 {
333 putc(ch);
334 }
335
vprintf(const char * fmt,va_list va)336 int vprintf(const char *fmt, va_list va)
337 {
338 struct printf_info info;
339
340 info.putc = putc_normal;
341 return _vprintf(&info, fmt, va);
342 }
343
printf(const char * fmt,...)344 int printf(const char *fmt, ...)
345 {
346 struct printf_info info;
347
348 va_list va;
349 int ret;
350
351 info.putc = putc_normal;
352 va_start(va, fmt);
353 ret = _vprintf(&info, fmt, va);
354 va_end(va);
355
356 return ret;
357 }
358 #endif
359
putc_outstr(struct printf_info * info,char ch)360 static void putc_outstr(struct printf_info *info, char ch)
361 {
362 *info->outstr++ = ch;
363 }
364
sprintf(char * buf,const char * fmt,...)365 int sprintf(char *buf, const char *fmt, ...)
366 {
367 struct printf_info info;
368 va_list va;
369 int ret;
370
371 va_start(va, fmt);
372 info.outstr = buf;
373 info.putc = putc_outstr;
374 ret = _vprintf(&info, fmt, va);
375 va_end(va);
376 *info.outstr = '\0';
377
378 return ret;
379 }
380
381 #if CONFIG_IS_ENABLED(LOG)
382 /* Note that size is ignored */
vsnprintf(char * buf,size_t size,const char * fmt,va_list va)383 int vsnprintf(char *buf, size_t size, const char *fmt, va_list va)
384 {
385 struct printf_info info;
386 int ret;
387
388 info.outstr = buf;
389 info.putc = putc_outstr;
390 ret = _vprintf(&info, fmt, va);
391 *info.outstr = '\0';
392
393 return ret;
394 }
395 #endif
396
397 /* Note that size is ignored */
snprintf(char * buf,size_t size,const char * fmt,...)398 int snprintf(char *buf, size_t size, const char *fmt, ...)
399 {
400 struct printf_info info;
401 va_list va;
402 int ret;
403
404 va_start(va, fmt);
405 info.outstr = buf;
406 info.putc = putc_outstr;
407 ret = _vprintf(&info, fmt, va);
408 va_end(va);
409 *info.outstr = '\0';
410
411 return ret;
412 }
413
print_grouped_ull(unsigned long long int_val,int digits)414 void print_grouped_ull(unsigned long long int_val, int digits)
415 {
416 /* Don't try to print the upper 32-bits */
417 printf("%ld ", (ulong)int_val);
418 }
419