1/* 2 * Copyright 2014, General Dynamics C4 Systems 3 * 4 * SPDX-License-Identifier: GPL-2.0-only 5 * 6 * Portions derived from musl: 7 * 8 * Copyright �� 2005-2020 Rich Felker, et al. 9 * 10 * SPDX-License-Identifier: MIT 11 */ 12 13#include <config.h> 14#include <machine/io.h> 15 16#ifdef CONFIG_PRINTING 17 18#include <stdarg.h> 19#include <stdint.h> 20 21/* 22 * a handle defining how to output a character 23 */ 24typedef void (*out_fn)(char character, char *buf, word_t idx); 25 26/* 27 * structure to allow a generic vprintf 28 * a out_fn handle and a buf to work on 29 */ 30typedef struct { 31 out_fn putchar; 32 char *buf; 33 word_t idx; 34 word_t maxlen; 35} out_wrap_t; 36 37/* 38 * putchar would then just call the handle with its buf 39 * and current idx and then increment idx 40 */ 41static void putchar_wrap(out_wrap_t *out, char c) 42{ 43 if (out->maxlen < 0 || out->idx < out->maxlen) { 44 out->putchar(c, out->buf, out->idx); 45 out->idx++; 46 } 47} 48 49 50void putchar(char c) 51{ 52 if (c == '\n') { 53 putDebugChar('\r'); 54 } 55 putDebugChar(c); 56} 57 58static inline bool_t isdigit(char c) 59{ 60 return c >= '0' && 61 c <= '9'; 62} 63 64/* Convenient bit representation for modifier flags, which all fall 65 * within 31 codepoints of the space character. */ 66 67#define MASK_TYPE(a) (1U<<( a -' ')) 68 69#define ALT_FORM (1U<<('#'-' ')) 70#define ZERO_PAD (1U<<('0'-' ')) 71#define LEFT_ADJ (1U<<('-'-' ')) 72#define PAD_POS (1U<<(' '-' ')) 73#define MARK_POS (1U<<('+'-' ')) 74#define GROUPED (1U<<('\''-' ')) 75 76#define FLAGMASK (ALT_FORM|ZERO_PAD|LEFT_ADJ|PAD_POS|MARK_POS|GROUPED) 77 78#define INTMAX_MAX INT32_MAX 79#define INT_MAX 0x7fffffff 80 81#define ULONG_MAX ((unsigned long)(-1)) 82 83/* State machine to accept length modifiers + conversion specifiers. 84 * Result is 0 on failure, or an argument type to pop on success. */ 85 86enum { 87 BARE, LPRE, LLPRE, HPRE, HHPRE, BIGLPRE, 88 ZTPRE, JPRE, 89 STOP, 90 PTR, INT, UINT, ULLONG, 91 LONG, ULONG, 92 SHORT, USHORT, CHAR, UCHAR, 93 WORDT, LLONG, 94#define IMAX LLONG 95#define UMAX ULLONG 96#define PDIFF LONG 97#define UIPTR ULONG 98 NOARG, 99 MAXSTATE 100}; 101 102#define S(x) [(x)-'A'] 103 104static const unsigned char states[]['z' - 'A' + 1] = { 105 { /* 0: bare types */ 106 S('d') = INT, S('i') = INT, 107 S('o') = UINT, S('u') = UINT, S('x') = UINT, S('X') = UINT, 108 S('c') = CHAR, 109 S('s') = PTR, S('p') = UIPTR, S('n') = PTR, 110 S('l') = LPRE, S('h') = HPRE, 111 S('z') = ZTPRE, S('j') = JPRE, S('t') = ZTPRE, 112 }, { /* 1: l-prefixed */ 113 S('d') = LONG, S('i') = LONG, 114 S('o') = ULONG, S('u') = ULONG, S('x') = ULONG, S('X') = ULONG, 115 S('n') = PTR, 116 S('l') = LLPRE, 117 }, { /* 2: ll-prefixed */ 118 S('d') = LLONG, S('i') = LLONG, 119 S('o') = ULLONG, S('u') = ULLONG, 120 S('x') = ULLONG, S('X') = ULLONG, 121 S('n') = PTR, 122 }, { /* 3: h-prefixed */ 123 S('d') = SHORT, S('i') = SHORT, 124 S('o') = USHORT, S('u') = USHORT, 125 S('x') = USHORT, S('X') = USHORT, 126 S('n') = PTR, 127 S('h') = HHPRE, 128 }, { /* 4: hh-prefixed */ 129 S('d') = CHAR, S('i') = CHAR, 130 S('o') = UCHAR, S('u') = UCHAR, 131 S('x') = UCHAR, S('X') = UCHAR, 132 S('n') = PTR, 133 }, { /* 5: L-prefixed not supported */ 134 }, { /* 6: z- or t-prefixed (assumed to be same size) */ 135 S('d') = PDIFF, S('i') = PDIFF, 136 S('o') = WORDT, S('u') = WORDT, 137 S('x') = WORDT, S('X') = WORDT, 138 S('n') = PTR, 139 }, { /* 7: j-prefixed */ 140 S('d') = IMAX, S('i') = IMAX, 141 S('o') = UMAX, S('u') = UMAX, 142 S('x') = UMAX, S('X') = UMAX, 143 S('n') = PTR, 144 } 145}; 146 147#define OOB(x) ((unsigned)(x)-'A' > 'z'-'A') 148#define DIGIT(c) (c - '0') 149 150union arg { 151 word_t i; 152 long double f; 153 void *p; 154}; 155 156static void pop_arg(union arg *arg, int type, va_list *ap) 157{ 158 switch (type) { 159 case PTR: 160 arg->p = va_arg(*ap, void *); 161 break; 162 case INT: 163 arg->i = va_arg(*ap, int); 164 break; 165 case UINT: 166 arg->i = va_arg(*ap, unsigned int); 167 break; 168 case LONG: 169 arg->i = va_arg(*ap, long); 170 break; 171 case ULONG: 172 arg->i = va_arg(*ap, unsigned long); 173 break; 174 case LLONG: 175 arg->i = va_arg(*ap, long long); 176 break; 177 case ULLONG: 178 arg->i = va_arg(*ap, unsigned long long); 179 break; 180 case SHORT: 181 arg->i = (short)va_arg(*ap, int); 182 break; 183 case USHORT: 184 arg->i = (unsigned short)va_arg(*ap, int); 185 break; 186 case CHAR: 187 arg->i = (signed char)va_arg(*ap, int); 188 break; 189 case UCHAR: 190 arg->i = (unsigned char)va_arg(*ap, int); 191 break; 192 case WORDT: 193 arg->i = va_arg(*ap, word_t); 194 } 195} 196 197static void out(out_wrap_t *f, const char *s, word_t l) 198{ 199 for (word_t i = 0; i < l; i++) { 200 putchar_wrap(f, s[i]); 201 } 202} 203 204static void pad(out_wrap_t *f, char c, int w, int l, int fl) 205{ 206 char pad[256]; 207 if (fl & (LEFT_ADJ | ZERO_PAD) || l >= w) { 208 return; 209 } 210 l = w - l; 211 memset(pad, c, l > sizeof pad ? sizeof pad : l); 212 for (; l >= sizeof pad; l -= sizeof pad) { 213 out(f, pad, sizeof pad); 214 } 215 out(f, pad, l); 216} 217 218static const char xdigits[16] = { 219 "0123456789ABCDEF" 220}; 221 222static char *fmt_x(word_t x, char *s, int lower) 223{ 224 for (; x; x >>= 4) { 225 *--s = xdigits[(x & 15)] | lower; 226 } 227 return s; 228} 229 230static char *fmt_o(word_t x, char *s) 231{ 232 for (; x; x >>= 3) { 233 *--s = '0' + (x & 7); 234 } 235 return s; 236} 237 238static char *fmt_u(word_t x, char *s) 239{ 240 unsigned long y; 241 for (; x > ULONG_MAX; x /= 10) { 242 *--s = '0' + x % 10; 243 } 244 for (y = x; y; y /= 10) { 245 *--s = '0' + y % 10; 246 } 247 return s; 248} 249 250// Maximum buffer size taken to ensure correct adaptation 251// However, it could be reduced/removed if we could measure 252// the buf length under all code paths 253#define LDBL_MANT_DIG 113 254 255#define NL_ARGMAX 9 256 257static int getint(char **s) 258{ 259 int i; 260 for (i = 0; isdigit(**s); (*s)++) { 261 if (i > INT_MAX / 10U || DIGIT(**s) > INT_MAX - 10 * i) { 262 i = -1; 263 } else { 264 i = 10 * i + DIGIT(**s); 265 } 266 } 267 return i; 268} 269 270static int printf_core(out_wrap_t *f, const char *fmt, va_list *ap, union arg *nl_arg, int *nl_type) 271{ 272 char *a, *z, *s = (char *)fmt; 273 unsigned l10n = 0, fl; 274 int w, p, xp; 275 union arg arg; 276 int argpos; 277 unsigned st, ps; 278 int cnt = 0, l = 0; 279 word_t i; 280 char buf[sizeof(word_t) * 3 + 3 + LDBL_MANT_DIG / 4]; 281 const char *prefix; 282 int t, pl; 283 284 for (;;) { 285 /* This error is only specified for snprintf, but since it's 286 * unspecified for other forms, do the same. Stop immediately 287 * on overflow; otherwise %n could produce wrong results. */ 288 if (l > INT_MAX - cnt) { 289 goto overflow; 290 } 291 292 /* Update output count, end loop when fmt is exhausted */ 293 cnt += l; 294 if (!*s) { 295 break; 296 } 297 298 /* Handle literal text and %% format specifiers */ 299 for (a = s; *s && *s != '%'; s++); 300 for (z = s; s[0] == '%' && s[1] == '%'; z++, s += 2); 301 if (z - a > INT_MAX - cnt) { 302 goto overflow; 303 } 304 l = z - a; 305 if (f) { 306 out(f, a, l); 307 } 308 if (l) { 309 continue; 310 } 311 312 if (isdigit(s[1]) && s[2] == '$') { 313 l10n = 1; 314 argpos = DIGIT(s[1]); 315 s += 3; 316 } else { 317 argpos = -1; 318 s++; 319 } 320 321 /* Read modifier flags */ 322 for (fl = 0; (unsigned)*s - ' ' < 32 && (FLAGMASK & MASK_TYPE(*s)); s++) { 323 fl |= MASK_TYPE(*s); 324 } 325 326 /* Read field width */ 327 if (*s == '*') { 328 if (isdigit(s[1]) && s[2] == '$') { 329 l10n = 1; 330 nl_type[DIGIT(s[1])] = INT; 331 w = nl_arg[DIGIT(s[1])].i; 332 s += 3; 333 } else if (!l10n) { 334 w = f ? va_arg(*ap, int) : 0; 335 s++; 336 } else { 337 goto inval; 338 } 339 if (w < 0) { 340 fl |= LEFT_ADJ; 341 w = -w; 342 } 343 } else if ((w = getint(&s)) < 0) { 344 goto overflow; 345 } 346 347 /* Read precision */ 348 if (*s == '.' && s[1] == '*') { 349 if (isdigit(s[2]) && s[3] == '$') { 350 nl_type[DIGIT(s[2])] = INT; 351 p = nl_arg[DIGIT(s[2])].i; 352 s += 4; 353 } else if (!l10n) { 354 p = f ? va_arg(*ap, int) : 0; 355 s += 2; 356 } else { 357 goto inval; 358 } 359 xp = (p >= 0); 360 } else if (*s == '.') { 361 s++; 362 p = getint(&s); 363 xp = 1; 364 } else { 365 p = -1; 366 xp = 0; 367 } 368 369 /* Format specifier state machine */ 370 st = 0; 371 do { 372 if (OOB(*s)) { 373 goto inval; 374 } 375 ps = st; 376 st = states[st]S(*s++); 377 } while (st - 1 < STOP); 378 if (!st) { 379 goto inval; 380 } 381 382 /* Check validity of argument type (nl/normal) */ 383 if (st == NOARG) { 384 if (argpos >= 0) { 385 goto inval; 386 } 387 } else { 388 if (argpos >= 0) { 389 nl_type[argpos] = st; 390 arg = nl_arg[argpos]; 391 } else if (f) { 392 pop_arg(&arg, st, ap); 393 } else { 394 return 0; 395 } 396 } 397 398 if (!f) { 399 continue; 400 } 401 402 z = buf + sizeof(buf); 403 prefix = "-+ 0X0x"; 404 pl = 0; 405 t = s[-1]; 406 407 /* - and 0 flags are mutually exclusive */ 408 if (fl & LEFT_ADJ) { 409 fl &= ~ZERO_PAD; 410 } 411 412 if (t == 'n') { 413 if (!arg.p) { 414 continue; 415 } 416 switch (ps) { 417 case BARE: 418 *(int *)arg.p = cnt; 419 break; 420 case LPRE: 421 *(long *)arg.p = cnt; 422 break; 423 case LLPRE: 424 *(long long *)arg.p = cnt; 425 break; 426 case HPRE: 427 *(unsigned short *)arg.p = cnt; 428 break; 429 case HHPRE: 430 *(unsigned char *)arg.p = cnt; 431 break; 432 case ZTPRE: 433 *(word_t *)arg.p = cnt; 434 break; 435 case JPRE: 436 *(word_t *)arg.p = cnt; 437 break; 438 } 439 continue; 440 } else if (t == 'c') { 441 p = 1; 442 a = z - p; 443 *a = arg.i; 444 fl &= ~ZERO_PAD; 445 } else if (t == 's') { 446 a = arg.p ? arg.p : "(null)"; 447 z = a + strnlen(a, p < 0 ? INT_MAX : p); 448 if (p < 0 && *z) { 449 goto overflow; 450 } 451 p = z - a; 452 fl &= ~ZERO_PAD; 453 } else { 454 switch (t) { 455 case 'p': 456 p = MAX(p, 2 * sizeof(void *)); 457 t = 'x'; 458 fl |= ALT_FORM; 459 case 'x': 460 case 'X': 461 a = fmt_x(arg.i, z, t & 32); 462 if (arg.i && (fl & ALT_FORM)) { 463 prefix += (t >> 4); 464 pl = 2; 465 } 466 break; 467 case 'o': 468 a = fmt_o(arg.i, z); 469 if ((fl & ALT_FORM) && p < (z - a + 1)) { 470 p = z - a + 1; 471 } 472 break; 473 case 'd': 474 case 'i': 475 pl = 1; 476 if (arg.i > INTMAX_MAX) { 477 arg.i = -arg.i; 478 } else if (fl & MARK_POS) { 479 prefix++; 480 } else if (fl & PAD_POS) { 481 prefix += 2; 482 } else { 483 pl = 0; 484 } 485 case 'u': 486 a = fmt_u(arg.i, z); 487 break; 488 } 489 if (xp && p < 0) { 490 goto overflow; 491 } 492 if (xp) { 493 fl &= ~ZERO_PAD; 494 } 495 if (!arg.i && !p) { 496 a = z; 497 } else { 498 p = MAX(p, z - a + !arg.i); 499 } 500 } 501 502 if (p < z - a) { 503 p = z - a; 504 } 505 if (p > INT_MAX - pl) { 506 goto overflow; 507 } 508 if (w < pl + p) { 509 w = pl + p; 510 } 511 if (w > INT_MAX - cnt) { 512 goto overflow; 513 } 514 515 pad(f, ' ', w, pl + p, fl); 516 out(f, prefix, pl); 517 pad(f, '0', w, pl + p, fl ^ ZERO_PAD); 518 pad(f, '0', p, z - a, 0); 519 out(f, a, z - a); 520 pad(f, ' ', w, pl + p, fl ^ LEFT_ADJ); 521 522 l = w; 523 } 524 525 if (f) { 526 return cnt; 527 } 528 if (!l10n) { 529 return 0; 530 } 531 532 for (i = 1; i <= NL_ARGMAX && nl_type[i]; i++) { 533 pop_arg(nl_arg + i, nl_type[i], ap); 534 } 535 for (; i <= NL_ARGMAX && !nl_type[i]; i++); 536 if (i <= NL_ARGMAX) { 537 goto inval; 538 } 539 return 1; 540 541// goto for potential debug error support 542inval: 543overflow: 544 return -1; 545} 546 547// sprintf fills its buf with the given character 548static void buf_out_fn(char c, char *buf, word_t idx) 549{ 550 buf[idx] = c; 551} 552 553// printf only needs to call kernel_putchar 554static void kernel_out_fn(char c, char *buf, word_t idx) 555{ 556 kernel_putchar(c); 557} 558 559word_t puts(const char *s) 560{ 561 for (; *s; s++) { 562 kernel_putchar(*s); 563 } 564 kernel_putchar('\n'); 565 return 0; 566} 567 568static int vprintf(out_wrap_t *out, const char *fmt, va_list ap) 569{ 570 va_list ap2; 571 int nl_type[NL_ARGMAX + 1] = {0}; 572 union arg nl_arg[NL_ARGMAX + 1]; 573 int ret; 574 575 // validate format string 576 va_copy(ap2, ap); 577 if (printf_core(0, fmt, &ap2, nl_arg, nl_type) < 0) { 578 va_end(ap2); 579 return -1; 580 } 581 582 ret = printf_core(out, fmt, &ap2, nl_arg, nl_type); 583 va_end(ap2); 584 return ret; 585} 586 587word_t kprintf(const char *format, ...) 588{ 589 va_list args; 590 word_t ret; 591 592 out_wrap_t out = { kernel_out_fn, NULL, 0, -1 }; 593 594 va_start(args, format); 595 ret = vprintf(&out, format, args); 596 va_end(args); 597 return ret; 598} 599 600word_t ksnprintf(char *str, word_t size, const char *format, ...) 601{ 602 va_list args; 603 word_t i; 604 605 out_wrap_t out = { buf_out_fn, str, 0, size }; 606 607 va_start(args, format); 608 i = vprintf(&out, format, args); 609 va_end(args); 610 611 // make sure there is space for a 0 byte 612 if (i >= size) { 613 i = size - 1; 614 } 615 str[i] = 0; 616 617 return i; 618} 619 620#endif /* CONFIG_PRINTING */ 621