drivers/firmware/efi/libstub/vsprintf.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* -*- linux-c -*- ------------------------------------------------------- *
   3  *
   4  *   Copyright (C) 1991, 1992 Linus Torvalds
   5  *   Copyright 2007 rPath, Inc. - All Rights Reserved
   6  *
   7  * ----------------------------------------------------------------------- */
   8
   9 /*
  10  * Oh, it's a waste of space, but oh-so-yummy for debugging.
  11  */
  12
  13 #include <linux/stdarg.h>
  14
  15 #include <linux/compiler.h>
  16 #include <linux/ctype.h>
  17 #include <linux/kernel.h>
  18 #include <linux/limits.h>
  19 #include <linux/string.h>
  20 #include <linux/types.h>
  21
  22 static
  23 int skip_atoi(const char **s)
  24 {
  25         int i = 0;
  26
  27         while (isdigit(**s))
  28                 i = i * 10 + *((*s)++) - '0';
  29         return i;
  30 }
  31
  32 /*
  33  * put_dec_full4 handles numbers in the range 0 <= r < 10000.
  34  * The multiplier 0xccd is round(2^15/10), and the approximation
  35  * r/10 == (r * 0xccd) >> 15 is exact for all r < 16389.
  36  */
  37 static
  38 void put_dec_full4(char *end, unsigned int r)
  39 {
  40         int i;
  41
  42         for (i = 0; i < 3; i++) {
  43                 unsigned int q = (r * 0xccd) >> 15;
  44                 *--end = '0' + (r - q * 10);
  45                 r = q;
  46         }
  47         *--end = '0' + r;
  48 }
  49
  50 /* put_dec is copied from lib/vsprintf.c with small modifications */
  51
  52 /*
  53  * Call put_dec_full4 on x % 10000, return x / 10000.
  54  * The approximation x/10000 == (x * 0x346DC5D7) >> 43
  55  * holds for all x < 1,128,869,999.  The largest value this
  56  * helper will ever be asked to convert is 1,125,520,955.
  57  * (second call in the put_dec code, assuming n is all-ones).
  58  */
  59 static
  60 unsigned int put_dec_helper4(char *end, unsigned int x)
  61 {
  62         unsigned int q = (x * 0x346DC5D7ULL) >> 43;
  63
  64         put_dec_full4(end, x - q * 10000);
  65         return q;
  66 }
  67
  68 /* Based on code by Douglas W. Jones found at
  69  * <http://www.cs.uiowa.edu/~jones/bcd/decimal.html#sixtyfour>
  70  * (with permission from the author).
  71  * Performs no 64-bit division and hence should be fast on 32-bit machines.
  72  */
  73 static
  74 char *put_dec(char *end, unsigned long long n)
  75 {
  76         unsigned int d3, d2, d1, q, h;
  77         char *p = end;
  78
  79         d1  = ((unsigned int)n >> 16); /* implicit "& 0xffff" */
  80         h   = (n >> 32);
  81         d2  = (h      ) & 0xffff;
  82         d3  = (h >> 16); /* implicit "& 0xffff" */
  83
  84         /* n = 2^48 d3 + 2^32 d2 + 2^16 d1 + d0
  85              = 281_4749_7671_0656 d3 + 42_9496_7296 d2 + 6_5536 d1 + d0 */
  86         q = 656 * d3 + 7296 * d2 + 5536 * d1 + ((unsigned int)n & 0xffff);
  87         q = put_dec_helper4(p, q);
  88         p -= 4;
  89
  90         q += 7671 * d3 + 9496 * d2 + 6 * d1;
  91         q = put_dec_helper4(p, q);
  92         p -= 4;
  93
  94         q += 4749 * d3 + 42 * d2;
  95         q = put_dec_helper4(p, q);
  96         p -= 4;
  97
  98         q += 281 * d3;
  99         q = put_dec_helper4(p, q);
 100         p -= 4;
 101
 102         put_dec_full4(p, q);
 103         p -= 4;
 104
 105         /* strip off the extra 0's we printed */
 106         while (p < end && *p == '0')
 107                 ++p;
 108
 109         return p;
 110 }
 111
 112 static
 113 char *number(char *end, unsigned long long num, int base, char locase)
 114 {
 115         /*
 116          * locase = 0 or 0x20. ORing digits or letters with 'locase'
 117          * produces same digits or (maybe lowercased) letters
 118          */
 119
 120         /* we are called with base 8, 10 or 16, only, thus don't need "G..."  */
 121         static const char digits[16] = "0123456789ABCDEF"; /* "GHIJKLMNOPQRSTUVWXYZ"; */
 122
 123         switch (base) {
 124         case 10:
 125                 if (num != 0)
 126                         end = put_dec(end, num);
 127                 break;
 128         case 8:
 129                 for (; num != 0; num >>= 3)
 130                         *--end = '0' + (num & 07);
 131                 break;
 132         case 16:
 133                 for (; num != 0; num >>= 4)
 134                         *--end = digits[num & 0xf] | locase;
 135                 break;
 136         default:
 137                 unreachable();
 138         }
 139
 140         return end;
 141 }
 142
 143 #define ZEROPAD 1               /* pad with zero */
 144 #define SIGN    2               /* unsigned/signed long */
 145 #define PLUS    4               /* show plus */
 146 #define SPACE   8               /* space if plus */
 147 #define LEFT    16              /* left justified */
 148 #define SMALL   32              /* Must be 32 == 0x20 */
 149 #define SPECIAL 64              /* 0x */
 150 #define WIDE    128             /* UTF-16 string */
 151
 152 static
 153 int get_flags(const char **fmt)
 154 {
 155         int flags = 0;
 156
 157         do {
 158                 switch (**fmt) {
 159                 case '-':
 160                         flags |= LEFT;
 161                         break;
 162                 case '+':
 163                         flags |= PLUS;
 164                         break;
 165                 case ' ':
 166                         flags |= SPACE;
 167                         break;
 168                 case '#':
 169                         flags |= SPECIAL;
 170                         break;
 171                 case '0':
 172                         flags |= ZEROPAD;
 173                         break;
 174                 default:
 175                         return flags;
 176                 }
 177                 ++(*fmt);
 178         } while (1);
 179 }
 180
 181 static
 182 int get_int(const char **fmt, va_list *ap)
 183 {
 184         if (isdigit(**fmt))
 185                 return skip_atoi(fmt);
 186         if (**fmt == '*') {
 187                 ++(*fmt);
 188                 /* it's the next argument */
 189                 return va_arg(*ap, int);
 190         }
 191         return 0;
 192 }
 193
 194 static
 195 unsigned long long get_number(int sign, int qualifier, va_list *ap)
 196 {
 197         if (sign) {
 198                 switch (qualifier) {
 199                 case 'L':
 200                         return va_arg(*ap, long long);
 201                 case 'l':
 202                         return va_arg(*ap, long);
 203                 case 'h':
 204                         return (short)va_arg(*ap, int);
 205                 case 'H':
 206                         return (signed char)va_arg(*ap, int);
 207                 default:
 208                         return va_arg(*ap, int);
 209                 };
 210         } else {
 211                 switch (qualifier) {
 212                 case 'L':
 213                         return va_arg(*ap, unsigned long long);
 214                 case 'l':
 215                         return va_arg(*ap, unsigned long);
 216                 case 'h':
 217                         return (unsigned short)va_arg(*ap, int);
 218                 case 'H':
 219                         return (unsigned char)va_arg(*ap, int);
 220                 default:
 221                         return va_arg(*ap, unsigned int);
 222                 }
 223         }
 224 }
 225
 226 static
 227 char get_sign(long long *num, int flags)
 228 {
 229         if (!(flags & SIGN))
 230                 return 0;
 231         if (*num < 0) {
 232                 *num = -(*num);
 233                 return '-';
 234         }
 235         if (flags & PLUS)
 236                 return '+';
 237         if (flags & SPACE)
 238                 return ' ';
 239         return 0;
 240 }
 241
 242 static
 243 size_t utf16s_utf8nlen(const u16 *s16, size_t maxlen)
 244 {
 245         size_t len, clen;
 246
 247         for (len = 0; len < maxlen && *s16; len += clen) {
 248                 u16 c0 = *s16++;
 249
 250                 /* First, get the length for a BMP character */
 251                 clen = 1 + (c0 >= 0x80) + (c0 >= 0x800);
 252                 if (len + clen > maxlen)
 253                         break;
 254                 /*
 255                  * If this is a high surrogate, and we're already at maxlen, we
 256                  * can't include the character if it's a valid surrogate pair.
 257                  * Avoid accessing one extra word just to check if it's valid
 258                  * or not.
 259                  */
 260                 if ((c0 & 0xfc00) == 0xd800) {
 261                         if (len + clen == maxlen)
 262                                 break;
 263                         if ((*s16 & 0xfc00) == 0xdc00) {
 264                                 ++s16;
 265                                 ++clen;
 266                         }
 267                 }
 268         }
 269
 270         return len;
 271 }
 272
 273 static
 274 u32 utf16_to_utf32(const u16 **s16)
 275 {
 276         u16 c0, c1;
 277
 278         c0 = *(*s16)++;
 279         /* not a surrogate */
 280         if ((c0 & 0xf800) != 0xd800)
 281                 return c0;
 282         /* invalid: low surrogate instead of high */
 283         if (c0 & 0x0400)
 284                 return 0xfffd;
 285         c1 = **s16;
 286         /* invalid: missing low surrogate */
 287         if ((c1 & 0xfc00) != 0xdc00)
 288                 return 0xfffd;
 289         /* valid surrogate pair */
 290         ++(*s16);
 291         return (0x10000 - (0xd800 << 10) - 0xdc00) + (c0 << 10) + c1;
 292 }
 293
 294 #define PUTC(c) \
 295 do {                            \
 296         if (pos < size)         \
 297                 buf[pos] = (c); \
 298         ++pos;                  \
 299 } while (0);
 300
 301 int vsnprintf(char *buf, size_t size, const char *fmt, va_list ap)
 302 {
 303         /* The maximum space required is to print a 64-bit number in octal */
 304         char tmp[(sizeof(unsigned long long) * 8 + 2) / 3];
 305         char *tmp_end = &tmp[ARRAY_SIZE(tmp)];
 306         long long num;
 307         int base;
 308         const char *s;
 309         size_t len, pos;
 310         char sign;
 311
 312         int flags;              /* flags to number() */
 313
 314         int field_width;        /* width of output field */
 315         int precision;          /* min. # of digits for integers; max
 316                                    number of chars for from string */
 317         int qualifier;          /* 'h', 'hh', 'l' or 'll' for integer fields */
 318
 319         va_list args;
 320
 321         /*
 322          * We want to pass our input va_list to helper functions by reference,
 323          * but there's an annoying edge case. If va_list was originally passed
 324          * to us by value, we could just pass &ap down to the helpers. This is
 325          * the case on, for example, X86_32.
 326          * However, on X86_64 (and possibly others), va_list is actually a
 327          * size-1 array containing a structure. Our function parameter ap has
 328          * decayed from T[1] to T*, and &ap has type T** rather than T(*)[1],
 329          * which is what will be expected by a function taking a va_list *
 330          * parameter.
 331          * One standard way to solve this mess is by creating a copy in a local
 332          * variable of type va_list and then passing a pointer to that local
 333          * copy instead, which is what we do here.
 334          */
 335         va_copy(args, ap);
 336
 337         for (pos = 0; *fmt; ++fmt) {
 338                 if (*fmt != '%' || *++fmt == '%') {
 339                         PUTC(*fmt);
 340                         continue;
 341                 }
 342
 343                 /* process flags */
 344                 flags = get_flags(&fmt);
 345
 346                 /* get field width */
 347                 field_width = get_int(&fmt, &args);
 348                 if (field_width < 0) {
 349                         field_width = -field_width;
 350                         flags |= LEFT;
 351                 }
 352
 353                 if (flags & LEFT)
 354                         flags &= ~ZEROPAD;
 355
 356                 /* get the precision */
 357                 precision = -1;
 358                 if (*fmt == '.') {
 359                         ++fmt;
 360                         precision = get_int(&fmt, &args);
 361                         if (precision >= 0)
 362                                 flags &= ~ZEROPAD;
 363                 }
 364
 365                 /* get the conversion qualifier */
 366                 qualifier = -1;
 367                 if (*fmt == 'h' || *fmt == 'l') {
 368                         qualifier = *fmt;
 369                         ++fmt;
 370                         if (qualifier == *fmt) {
 371                                 qualifier -= 'a'-'A';
 372                                 ++fmt;
 373                         }
 374                 }
 375
 376                 sign = 0;
 377
 378                 switch (*fmt) {
 379                 case 'c':
 380                         flags &= LEFT;
 381                         s = tmp;
 382                         if (qualifier == 'l') {
 383                                 ((u16 *)tmp)[0] = (u16)va_arg(args, unsigned int);
 384                                 ((u16 *)tmp)[1] = L'\0';
 385                                 precision = INT_MAX;
 386                                 goto wstring;
 387                         } else {
 388                                 tmp[0] = (unsigned char)va_arg(args, int);
 389                                 precision = len = 1;
 390                         }
 391                         goto output;
 392
 393                 case 's':
 394                         flags &= LEFT;
 395                         if (precision < 0)
 396                                 precision = INT_MAX;
 397                         s = va_arg(args, void *);
 398                         if (!s)
 399                                 s = precision < 6 ? "" : "(null)";
 400                         else if (qualifier == 'l') {
 401                 wstring:
 402                                 flags |= WIDE;
 403                                 precision = len = utf16s_utf8nlen((const u16 *)s, precision);
 404                                 goto output;
 405                         }
 406                         precision = len = strnlen(s, precision);
 407                         goto output;
 408
 409                         /* integer number formats - set up the flags and "break" */
 410                 case 'o':
 411                         base = 8;
 412                         break;
 413
 414                 case 'p':
 415                         if (precision < 0)
 416                                 precision = 2 * sizeof(void *);
 417                         fallthrough;
 418                 case 'x':
 419                         flags |= SMALL;
 420                         fallthrough;
 421                 case 'X':
 422                         base = 16;
 423                         break;
 424
 425                 case 'd':
 426                 case 'i':
 427                         flags |= SIGN;
 428                         fallthrough;
 429                 case 'u':
 430                         flags &= ~SPECIAL;
 431                         base = 10;
 432                         break;
 433
 434                 default:
 435                         /*
 436                          * Bail out if the conversion specifier is invalid.
 437                          * There's probably a typo in the format string and the
 438                          * remaining specifiers are unlikely to match up with
 439                          * the arguments.
 440                          */
 441                         goto fail;
 442                 }
 443                 if (*fmt == 'p') {
 444                         num = (unsigned long)va_arg(args, void *);
 445                 } else {
 446                         num = get_number(flags & SIGN, qualifier, &args);
 447                 }
 448
 449                 sign = get_sign(&num, flags);
 450                 if (sign)
 451                         --field_width;
 452
 453                 s = number(tmp_end, num, base, flags & SMALL);
 454                 len = tmp_end - s;
 455                 /* default precision is 1 */
 456                 if (precision < 0)
 457                         precision = 1;
 458                 /* precision is minimum number of digits to print */
 459                 if (precision < len)
 460                         precision = len;
 461                 if (flags & SPECIAL) {
 462                         /*
 463                          * For octal, a leading 0 is printed only if necessary,
 464                          * i.e. if it's not already there because of the
 465                          * precision.
 466                          */
 467                         if (base == 8 && precision == len)
 468                                 ++precision;
 469                         /*
 470                          * For hexadecimal, the leading 0x is skipped if the
 471                          * output is empty, i.e. both the number and the
 472                          * precision are 0.
 473                          */
 474                         if (base == 16 && precision > 0)
 475                                 field_width -= 2;
 476                         else
 477                                 flags &= ~SPECIAL;
 478                 }
 479                 /*
 480                  * For zero padding, increase the precision to fill the field
 481                  * width.
 482                  */
 483                 if ((flags & ZEROPAD) && field_width > precision)
 484                         precision = field_width;
 485
 486 output:
 487                 /* Calculate the padding necessary */
 488                 field_width -= precision;
 489                 /* Leading padding with ' ' */
 490                 if (!(flags & LEFT))
 491                         while (field_width-- > 0)
 492                                 PUTC(' ');
 493                 /* sign */
 494                 if (sign)
 495                         PUTC(sign);
 496                 /* 0x/0X for hexadecimal */
 497                 if (flags & SPECIAL) {
 498                         PUTC('0');
 499                         PUTC( 'X' | (flags & SMALL));
 500                 }
 501                 /* Zero padding and excess precision */
 502                 while (precision-- > len)
 503                         PUTC('0');
 504                 /* Actual output */
 505                 if (flags & WIDE) {
 506                         const u16 *ws = (const u16 *)s;
 507
 508                         while (len-- > 0) {
 509                                 u32 c32 = utf16_to_utf32(&ws);
 510                                 u8 *s8;
 511                                 size_t clen;
 512
 513                                 if (c32 < 0x80) {
 514                                         PUTC(c32);
 515                                         continue;
 516                                 }
 517
 518                                 /* Number of trailing octets */
 519                                 clen = 1 + (c32 >= 0x800) + (c32 >= 0x10000);
 520
 521                                 len -= clen;
 522                                 s8 = (u8 *)&buf[pos];
 523
 524                                 /* Avoid writing partial character */
 525                                 PUTC('\0');
 526                                 pos += clen;
 527                                 if (pos >= size)
 528                                         continue;
 529
 530                                 /* Set high bits of leading octet */
 531                                 *s8 = (0xf00 >> 1) >> clen;
 532                                 /* Write trailing octets in reverse order */
 533                                 for (s8 += clen; clen; --clen, c32 >>= 6)
 534                                         *s8-- = 0x80 | (c32 & 0x3f);
 535                                 /* Set low bits of leading octet */
 536                                 *s8 |= c32;
 537                         }
 538                 } else {
 539                         while (len-- > 0)
 540                                 PUTC(*s++);
 541                 }
 542                 /* Trailing padding with ' ' */
 543                 while (field_width-- > 0)
 544                         PUTC(' ');
 545         }
 546 fail:
 547         va_end(args);
 548
 549         if (size)
 550                 buf[min(pos, size-1)] = '\0';
 551
 552         return pos;
 553 }
 554
 555 int snprintf(char *buf, size_t size, const char *fmt, ...)
 556 {
 557         va_list args;
 558         int i;
 559
 560         va_start(args, fmt);
 561         i = vsnprintf(buf, size, fmt, args);
 562         va_end(args);
 563         return i;
 564 }