1 /*
2 * libc printf and friends
3 *
4 * This code is free software; you can redistribute it and/or modify it
5 * under the terms of the GNU Library General Public License version 2.
6 */
7
8 #include "libcflat.h"
9 #include "ctype.h"
10
11 #define BUFSZ 2000
12
13 typedef struct pstream {
14 char *buffer;
15 int remain;
16 int added;
17 } pstream_t;
18
19 typedef struct strprops {
20 char pad;
21 int npad;
22 bool alternate;
23 int precision;
24 } strprops_t;
25
addchar(pstream_t * p,char c)26 static void addchar(pstream_t *p, char c)
27 {
28 if (p->remain) {
29 *p->buffer++ = c;
30 --p->remain;
31 }
32 ++p->added;
33 }
34
print_str(pstream_t * p,const char * s,strprops_t props)35 static void print_str(pstream_t *p, const char *s, strprops_t props)
36 {
37 const char *s_orig = s;
38 int npad = props.npad;
39
40 if (npad > 0) {
41 npad -= strlen(s_orig);
42 while (npad > 0) {
43 addchar(p, props.pad);
44 --npad;
45 }
46 }
47
48 while (*s && props.precision--)
49 addchar(p, *s++);
50
51 if (npad < 0) {
52 props.pad = ' '; /* ignore '0' flag with '-' flag */
53 npad += strlen(s_orig);
54 while (npad < 0) {
55 addchar(p, props.pad);
56 ++npad;
57 }
58 }
59 }
60
61 /*
62 * Adapted from drivers/firmware/efi/libstub/vsprintf.c
63 */
utf16_to_utf32(const u16 ** s16)64 static u32 utf16_to_utf32(const u16 **s16)
65 {
66 u16 c0, c1;
67
68 c0 = *(*s16)++;
69 /* not a surrogate */
70 if ((c0 & 0xf800) != 0xd800)
71 return c0;
72 /* invalid: low surrogate instead of high */
73 if (c0 & 0x0400)
74 return 0xfffd;
75 c1 = **s16;
76 /* invalid: missing low surrogate */
77 if ((c1 & 0xfc00) != 0xdc00)
78 return 0xfffd;
79 /* valid surrogate pair */
80 ++(*s16);
81 return (0x10000 - (0xd800 << 10) - 0xdc00) + (c0 << 10) + c1;
82 }
83
84 /*
85 * Adapted from drivers/firmware/efi/libstub/vsprintf.c
86 */
utf16s_utf8nlen(const u16 * s16,size_t maxlen)87 static size_t utf16s_utf8nlen(const u16 *s16, size_t maxlen)
88 {
89 size_t len, clen;
90
91 for (len = 0; len < maxlen && *s16; len += clen) {
92 u16 c0 = *s16++;
93
94 /* First, get the length for a BMP character */
95 clen = 1 + (c0 >= 0x80) + (c0 >= 0x800);
96 if (len + clen > maxlen)
97 break;
98 /*
99 * If this is a high surrogate, and we're already at maxlen, we
100 * can't include the character if it's a valid surrogate pair.
101 * Avoid accessing one extra word just to check if it's valid
102 * or not.
103 */
104 if ((c0 & 0xfc00) == 0xd800) {
105 if (len + clen == maxlen)
106 break;
107 if ((*s16 & 0xfc00) == 0xdc00) {
108 ++s16;
109 ++clen;
110 }
111 }
112 }
113
114 return len;
115 }
116
117 /*
118 * Adapted from drivers/firmware/efi/libstub/vsprintf.c
119 */
print_wstring(pstream_t * p,const u16 * s,strprops_t props)120 static void print_wstring(pstream_t *p, const u16 *s, strprops_t props)
121 {
122 const u16 *ws = (const u16 *)s;
123 size_t pos = 0, size = p->remain + 1, len = utf16s_utf8nlen(ws, props.precision);
124
125 while (len-- > 0) {
126 u32 c32 = utf16_to_utf32(&ws);
127 u8 *s8;
128 size_t clen;
129
130 if (c32 < 0x80) {
131 addchar(p, c32);
132 continue;
133 }
134
135 /* Number of trailing octets */
136 clen = 1 + (c32 >= 0x800) + (c32 >= 0x10000);
137
138 len -= clen;
139 s8 = (u8 *)(p->buffer - p->added + pos);
140
141 /* Avoid writing partial character */
142 addchar(p, '\0');
143 pos += clen;
144 if (pos >= size)
145 continue;
146
147 /* Set high bits of leading octet */
148 *s8 = (0xf00 >> 1) >> clen;
149 /* Write trailing octets in reverse order */
150 for (s8 += clen; clen; --clen, c32 >>= 6)
151 *s8-- = 0x80 | (c32 & 0x3f);
152 /* Set low bits of leading octet */
153 *s8 |= c32;
154 }
155 }
156
157 static char digits[16] = "0123456789abcdef";
158
print_int(pstream_t * ps,long long n,int base,strprops_t props)159 static void print_int(pstream_t *ps, long long n, int base, strprops_t props)
160 {
161 char buf[sizeof(long) * 3 + 2], *p = buf;
162 int s = 0, i;
163
164 if (n < 0) {
165 n = -n;
166 s = 1;
167 }
168
169 while (n) {
170 *p++ = digits[n % base];
171 n /= base;
172 }
173
174 while (p == buf || (p - buf < props.precision))
175 *p++ = '0';
176 props.precision = -1;
177
178 if (s)
179 *p++ = '-';
180
181 for (i = 0; i < (p - buf) / 2; ++i) {
182 char tmp;
183
184 tmp = buf[i];
185 buf[i] = p[-1 - i];
186 p[-1 - i] = tmp;
187 }
188
189 *p = 0;
190
191 print_str(ps, buf, props);
192 }
193
print_unsigned(pstream_t * ps,unsigned long long n,int base,strprops_t props)194 static void print_unsigned(pstream_t *ps, unsigned long long n, int base,
195 strprops_t props)
196 {
197 char buf[sizeof(long) * 3 + 3], *p = buf;
198 int i;
199
200 while (n) {
201 *p++ = digits[n % base];
202 n /= base;
203 }
204
205 if (p == buf)
206 props.alternate = false;
207
208 while (p == buf || (p - buf < props.precision))
209 *p++ = '0';
210 props.precision = -1;
211
212 if (props.alternate && base == 16) {
213 if (props.pad == '0') {
214 addchar(ps, '0');
215 addchar(ps, 'x');
216
217 if (props.npad > 0)
218 props.npad = MAX(props.npad - 2, 0);
219 } else {
220 *p++ = 'x';
221 *p++ = '0';
222 }
223 }
224
225 for (i = 0; i < (p - buf) / 2; ++i) {
226 char tmp;
227
228 tmp = buf[i];
229 buf[i] = p[-1 - i];
230 p[-1 - i] = tmp;
231 }
232
233 *p = 0;
234
235 print_str(ps, buf, props);
236 }
237
fmtnum(const char ** fmt)238 static int fmtnum(const char **fmt)
239 {
240 const char *f = *fmt;
241 int len = 0, num;
242
243 if (*f == '-')
244 ++f, ++len;
245
246 while (*f >= '0' && *f <= '9')
247 ++f, ++len;
248
249 num = atol(*fmt);
250 *fmt += len;
251 return num;
252 }
253
254 /*
255 * Adapted from drivers/firmware/efi/libstub/vsprintf.c
256 */
skip_atoi(const char ** s)257 static int skip_atoi(const char **s)
258 {
259 int i = 0;
260
261 do {
262 i = i*10 + *((*s)++) - '0';
263 } while (isdigit(**s));
264
265 return i;
266 }
267
268 /*
269 * Adapted from drivers/firmware/efi/libstub/vsprintf.c
270 */
get_int(const char ** fmt,va_list * ap)271 static int get_int(const char **fmt, va_list *ap)
272 {
273 if (isdigit(**fmt))
274 return skip_atoi(fmt);
275
276 if (**fmt == '*') {
277 ++(*fmt);
278 /* it's the next argument */
279 return va_arg(*ap, int);
280 }
281 return 0;
282 }
283
vsnprintf(char * buf,int size,const char * fmt,va_list va)284 int vsnprintf(char *buf, int size, const char *fmt, va_list va)
285 {
286 pstream_t s;
287 va_list args;
288
289 /*
290 * We want to pass our input va_list to helper functions by reference,
291 * but there's an annoying edge case. If va_list was originally passed
292 * to us by value, we could just pass &ap down to the helpers. This is
293 * the case on, for example, X86_32.
294 * However, on X86_64 (and possibly others), va_list is actually a
295 * size-1 array containing a structure. Our function parameter ap has
296 * decayed from T[1] to T*, and &ap has type T** rather than T(*)[1],
297 * which is what will be expected by a function taking a va_list *
298 * parameter.
299 * One standard way to solve this mess is by creating a copy in a local
300 * variable of type va_list and then passing a pointer to that local
301 * copy instead, which is what we do here.
302 */
303 va_copy(args, va);
304
305 s.buffer = buf;
306 s.remain = size - 1;
307 s.added = 0;
308 while (*fmt) {
309 char f = *fmt++;
310 int nlong = 0;
311 strprops_t props;
312 memset(&props, 0, sizeof(props));
313 props.pad = ' ';
314 props.precision = -1;
315
316 if (f != '%') {
317 addchar(&s, f);
318 continue;
319 }
320 morefmt:
321 f = *fmt++;
322 switch (f) {
323 case '%':
324 addchar(&s, '%');
325 break;
326 case 'c':
327 addchar(&s, va_arg(args, int));
328 break;
329 case '\0':
330 --fmt;
331 break;
332 case '.':
333 props.pad = ' ';
334 props.precision = get_int(&fmt, &args);
335 goto morefmt;
336 case '#':
337 props.alternate = true;
338 goto morefmt;
339 case '0':
340 props.pad = '0';
341 ++fmt;
342 /* fall through */
343 case '1' ... '9':
344 case '-':
345 --fmt;
346 props.npad = fmtnum(&fmt);
347 goto morefmt;
348 case 'l':
349 ++nlong;
350 goto morefmt;
351 case 't':
352 case 'z':
353 /* Here we only care that sizeof(size_t) == sizeof(long).
354 * On a 32-bit platform it doesn't matter that size_t is
355 * typedef'ed to int or long; va_arg will work either way.
356 * Same for ptrdiff_t (%td).
357 */
358 nlong = 1;
359 goto morefmt;
360 case 'd':
361 switch (nlong) {
362 case 0:
363 print_int(&s, va_arg(args, int), 10, props);
364 break;
365 case 1:
366 print_int(&s, va_arg(args, long), 10, props);
367 break;
368 default:
369 print_int(&s, va_arg(args, long long), 10, props);
370 break;
371 }
372 break;
373 case 'u':
374 switch (nlong) {
375 case 0:
376 print_unsigned(&s, va_arg(args, unsigned int), 10, props);
377 break;
378 case 1:
379 print_unsigned(&s, va_arg(args, unsigned long), 10, props);
380 break;
381 default:
382 print_unsigned(&s, va_arg(args, unsigned long long), 10, props);
383 break;
384 }
385 break;
386 case 'x':
387 switch (nlong) {
388 case 0:
389 print_unsigned(&s, va_arg(args, unsigned int), 16, props);
390 break;
391 case 1:
392 print_unsigned(&s, va_arg(args, unsigned long), 16, props);
393 break;
394 default:
395 print_unsigned(&s, va_arg(args, unsigned long long), 16, props);
396 break;
397 }
398 break;
399 case 'p':
400 props.alternate = true;
401 print_unsigned(&s, (unsigned long)va_arg(args, void *), 16, props);
402 break;
403 case 's':
404 if (nlong)
405 print_wstring(&s, va_arg(args, const u16 *), props);
406 else
407 print_str(&s, va_arg(args, const char *), props);
408 break;
409 default:
410 addchar(&s, f);
411 break;
412 }
413 }
414 va_end(args);
415 *s.buffer = 0;
416 return s.added;
417 }
418
snprintf(char * buf,int size,const char * fmt,...)419 int snprintf(char *buf, int size, const char *fmt, ...)
420 {
421 va_list va;
422 int r;
423
424 va_start(va, fmt);
425 r = vsnprintf(buf, size, fmt, va);
426 va_end(va);
427 return r;
428 }
429
vprintf(const char * fmt,va_list va)430 int vprintf(const char *fmt, va_list va)
431 {
432 char buf[BUFSZ];
433 int r;
434
435 r = vsnprintf(buf, sizeof(buf), fmt, va);
436 puts(buf);
437 return r;
438 }
439
printf(const char * fmt,...)440 int printf(const char *fmt, ...)
441 {
442 va_list va;
443 char buf[BUFSZ];
444 int r;
445
446 va_start(va, fmt);
447 r = vsnprintf(buf, sizeof buf, fmt, va);
448 va_end(va);
449 puts(buf);
450 return r;
451 }
452
binstr(unsigned long x,char out[BINSTR_SZ])453 void binstr(unsigned long x, char out[BINSTR_SZ])
454 {
455 int i;
456 char *c;
457 int n;
458
459 n = sizeof(unsigned long) * 8;
460 i = 0;
461 c = &out[0];
462 for (;;) {
463 *c++ = (x & (1ul << (n - i - 1))) ? '1' : '0';
464 i++;
465
466 if (i == n) {
467 *c = '\0';
468 break;
469 }
470 if (i % 4 == 0)
471 *c++ = '\'';
472 }
473 assert(c + 1 - &out[0] == BINSTR_SZ);
474 }
475
print_binstr(unsigned long x)476 void print_binstr(unsigned long x)
477 {
478 char out[BINSTR_SZ];
479 binstr(x, out);
480 printf("%s", out);
481 }
482