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 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 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 */ 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 */ 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 */ 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 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 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 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 */ 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 */ 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 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 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 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 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 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 476 void print_binstr(unsigned long x) 477 { 478 char out[BINSTR_SZ]; 479 binstr(x, out); 480 printf("%s", out); 481 } 482