armsrc/printf.c

   1 //-----------------------------------------------------------------------------
   2 // Borrowed initially from subr_prf.c 8.3 (Berkeley) 1/21/94
   3 // Copyright (c) 1986, 1988, 1991, 1993
   4 // The Regents of the University of California.  All rights reserved.
   5 // (c) UNIX System Laboratories, Inc.
   6 // All or some portions of this file are derived from material licensed
   7 // to the University of California by American Telephone and Telegraph
   8 // Co. or Unix System Laboratories, Inc. and are reproduced herein with
   9 // the permission of UNIX System Laboratories, Inc.
  10 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
  11 //
  12 // This program is free software: you can redistribute it and/or modify
  13 // it under the terms of the GNU General Public License as published by
  14 // the Free Software Foundation, either version 3 of the License, or
  15 // (at your option) any later version.
  16 //
  17 // This program is distributed in the hope that it will be useful,
  18 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  19 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  20 // GNU General Public License for more details.
  21 //
  22 // See LICENSE.txt for the text of the license.
  23 //-----------------------------------------------------------------------------
  24 #include "printf.h"
  25 #include "string.h"
  26
  27 typedef unsigned char u_char;
  28 typedef unsigned int u_int;
  29 typedef unsigned long long u_quad_t;
  30 typedef long long quad_t;
  31 typedef unsigned long u_long;
  32 typedef unsigned short u_short;
  33 typedef int ssize_t;
  34
  35 #define NBBY    8               /* number of bits in a byte */
  36
  37 static char const hex2ascii_data[] = "0123456789abcdefghijklmnopqrstuvwxyz";
  38 #define hex2ascii(hex)  (hex2ascii_data[hex])
  39 #define toupper(c)      ((c) - 0x20 * (((c) >= 'a') && ((c) <= 'z')))
  40
  41 /* Max number conversion buffer length: a u_quad_t in base 2, plus NUL byte. */
  42 #define MAXNBUF (sizeof(intmax_t) * NBBY + 1)
  43
  44 /*
  45  * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
  46  * order; return an optional length and a pointer to the last character
  47  * written in the buffer (i.e., the first character of the string).
  48  * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
  49  */
  50 static char *
  51 ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper) {
  52     char *p = nbuf;
  53     *p = '\0';
  54     do {
  55         char c = hex2ascii(num % base);
  56         *++p = upper ? toupper(c) : c;
  57     } while (num /= base);
  58     if (lenp)
  59         *lenp = p - nbuf;
  60     return (p);
  61 }
  62
  63 /*
  64  * Scaled down version of printf(3).
  65  *
  66  * Two additional formats:
  67  *
  68  * The format %b is supported to decode error registers.
  69  * Its usage is:
  70  *
  71  * printf("reg=%b\n", regval, "*");
  72  *
  73  * where  is the output base expressed as a control character, e.g.
  74  * \10 gives octal; \20 gives hex.  Each arg is a sequence of characters,
  75  * the first of which gives the bit number to be inspected (origin 1), and
  76  * the next characters (up to a control character, i.e. a character <= 32),
  77  * give the name of the register.  Thus:
  78  *
  79  * kvprintf("reg=%b\n", 3, "\10\2BITTWO\1BITONE\n");
  80  *
  81  * would produce output:
  82  *
  83  * reg=3
  84  *
  85  * XXX:  %D  -- Hexdump, takes pointer and separator string:
  86  *  ("%6D", ptr, ":")   -> XX:XX:XX:XX:XX:XX
  87  *  ("%*D", len, ptr, " " -> XX XX XX XX ...
  88  */
  89 int
  90 kvsprintf(char const *fmt, void *arg, int radix, va_list ap) {
  91 #define PCHAR(c) {int cc=(c); *d++ = cc; retval++; }
  92     char nbuf[MAXNBUF];
  93     char *d;
  94     const char *p, *percent, *q;
  95     u_char *up;
  96     int ch, n;
  97     uintmax_t num;
  98     int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
  99     int cflag, hflag, jflag, tflag, zflag;
 100     int dwidth, upper;
 101     char padc;
 102     int stop = 0, retval = 0;
 103
 104     num = 0;
 105     d = (char *) arg;
 106
 107     if (fmt == NULL)
 108         fmt = "(fmt null)\n";
 109
 110     if (radix < 2 || radix > 36)
 111         radix = 10;
 112
 113     for (;;) {
 114         padc = ' ';
 115         width = 0;
 116
 117         while ((ch = (u_char) * fmt++) != '%' || stop) {
 118             PCHAR(ch);
 119             if (ch == '\0') {
 120                 return (retval);
 121             }
 122         }
 123
 124         percent = fmt - 1;
 125         qflag = 0;
 126         lflag = 0;
 127         ladjust = 0;
 128         sharpflag = 0;
 129         neg = 0;
 130         sign = 0;
 131         dot = 0;
 132         dwidth = 0;
 133         upper = 0;
 134         cflag = 0;
 135         hflag = 0;
 136         jflag = 0;
 137         tflag = 0;
 138         zflag = 0;
 139 reswitch:
 140         switch (ch = (u_char) * fmt++) {
 141             case '.':
 142                 dot = 1;
 143                 goto reswitch;
 144             case '#':
 145                 sharpflag = 1;
 146                 goto reswitch;
 147             case '+':
 148                 sign = 1;
 149                 goto reswitch;
 150             case '-':
 151                 ladjust = 1;
 152                 goto reswitch;
 153             case '%':
 154                 PCHAR(ch);
 155                 break;
 156             case '*':
 157                 if (!dot) {
 158                     width = va_arg(ap, int);
 159                     if (width < 0) {
 160                         ladjust = !ladjust;
 161                         width = -width;
 162                     }
 163                 } else {
 164                     dwidth = va_arg(ap, int);
 165                 }
 166                 goto reswitch;
 167             case '0':
 168                 if (!dot) {
 169                     padc = '0';
 170                     goto reswitch;
 171                 }
 172             case '1':
 173             case '2':
 174             case '3':
 175             case '4':
 176             case '5':
 177             case '6':
 178             case '7':
 179             case '8':
 180             case '9':
 181                 for (n = 0;; ++fmt) {
 182                     n = n * 10 + ch - '0';
 183                     ch = *fmt;
 184                     if (ch < '0' || ch > '9') {
 185                         break;
 186                     }
 187                 }
 188                 if (dot) {
 189                     dwidth = n;
 190                 } else {
 191                     width = n;
 192                 }
 193                 goto reswitch;
 194             case 'b':
 195                 num = (u_int)va_arg(ap, int);
 196                 p = va_arg(ap, char *);
 197                 for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;) {
 198                     PCHAR(*q--);
 199                 }
 200
 201                 if (num == 0) {
 202                     break;
 203                 }
 204
 205                 for (tmp = 0; *p;) {
 206                     n = *p++;
 207                     if (num & (1 << (n - 1))) {
 208                         PCHAR(tmp ? ',' : '<');
 209                         for (; (n = *p) > ' '; ++p) {
 210                             PCHAR(n);
 211                         }
 212                         tmp = 1;
 213                     } else {
 214                         for (; *p > ' '; ++p) {};
 215                     }
 216                 }
 217
 218                 if (tmp) {
 219                     PCHAR('>');
 220                 }
 221                 break;
 222             case 'c':
 223                 PCHAR(va_arg(ap, int));
 224                 break;
 225             case 'D':
 226                 up = va_arg(ap, u_char *);
 227                 p = va_arg(ap, char *);
 228                 if (!width) {
 229                     width = 16;
 230                 }
 231
 232                 while (width--) {
 233                     PCHAR(hex2ascii(*up >> 4));
 234                     PCHAR(hex2ascii(*up & 0x0f));
 235                     up++;
 236                     if (width) {
 237                         for (q = p; *q; q++) {
 238                             PCHAR(*q);
 239                         }
 240                     }
 241                 }
 242                 break;
 243             case 'd':
 244             case 'i':
 245                 base = 10;
 246                 sign = 1;
 247                 goto handle_sign;
 248             case 'h':
 249                 if (hflag) {
 250                     hflag = 0;
 251                     cflag = 1;
 252                 } else {
 253                     hflag = 1;
 254                 }
 255                 goto reswitch;
 256             case 'j':
 257                 jflag = 1;
 258                 goto reswitch;
 259             case 'l':
 260                 if (lflag) {
 261                     lflag = 0;
 262                     qflag = 1;
 263                 } else {
 264                     lflag = 1;
 265                 }
 266                 goto reswitch;
 267             case 'n':
 268                 if (jflag)
 269                     *(va_arg(ap, intmax_t *)) = retval;
 270                 else if (qflag)
 271                     *(va_arg(ap, quad_t *)) = retval;
 272                 else if (lflag)
 273                     *(va_arg(ap, long *)) = retval;
 274                 else if (zflag)
 275                     *(va_arg(ap, size_t *)) = retval;
 276                 else if (hflag)
 277                     *(va_arg(ap, short *)) = retval;
 278                 else if (cflag)
 279                     *(va_arg(ap, char *)) = retval;
 280                 else
 281                     *(va_arg(ap, int *)) = retval;
 282                 break;
 283             case 'o':
 284                 base = 8;
 285                 goto handle_nosign;
 286             case 'p':
 287                 base = 16;
 288                 sharpflag = (width == 0);
 289                 sign = 0;
 290                 num = (uintptr_t)va_arg(ap, void *);
 291                 goto number;
 292             case 'q':
 293                 qflag = 1;
 294                 goto reswitch;
 295             case 'r':
 296                 base = radix;
 297                 if (sign) {
 298                     goto handle_sign;
 299                 }
 300                 goto handle_nosign;
 301             case 's':
 302                 p = va_arg(ap, char *);
 303                 if (p == NULL) {
 304                     p = "(null)";
 305                 }
 306
 307                 if (!dot) {
 308                     n = strlen(p);
 309                 } else {
 310                     for (n = 0; n < dwidth && p[n]; n++) {};
 311                 }
 312
 313                 width -= n;
 314
 315                 if (!ladjust && width > 0) {
 316                     while (width--) {
 317                         PCHAR(padc);
 318                     }
 319                 }
 320                 while (n--) {
 321                     PCHAR(*p++);
 322                 }
 323
 324                 if (ladjust && width > 0) {
 325                     while (width--) {
 326                         PCHAR(padc);
 327                     }
 328                 }
 329                 break;
 330             case 't':
 331                 tflag = 1;
 332                 goto reswitch;
 333             case 'u':
 334                 base = 10;
 335                 goto handle_nosign;
 336             case 'X':
 337                 upper = 1;
 338             case 'x':
 339                 base = 16;
 340                 goto handle_nosign;
 341             case 'y':
 342                 base = 16;
 343                 sign = 1;
 344                 goto handle_sign;
 345             case 'z':
 346                 zflag = 1;
 347                 goto reswitch;
 348 handle_nosign:
 349                 sign = 0;
 350                 if (jflag)
 351                     num = va_arg(ap, uintmax_t);
 352                 else if (qflag)
 353                     num = va_arg(ap, u_quad_t);
 354                 else if (tflag)
 355                     num = va_arg(ap, ptrdiff_t);
 356                 else if (lflag)
 357                     num = va_arg(ap, u_long);
 358                 else if (zflag)
 359                     num = va_arg(ap, size_t);
 360                 else if (hflag)
 361                     num = (u_short)va_arg(ap, int);
 362                 else if (cflag)
 363                     num = (u_char)va_arg(ap, int);
 364                 else
 365                     num = va_arg(ap, u_int);
 366                 goto number;
 367 handle_sign:
 368                 if (jflag)
 369                     num = va_arg(ap, intmax_t);
 370                 else if (qflag)
 371                     num = va_arg(ap, quad_t);
 372                 else if (tflag)
 373                     num = va_arg(ap, ptrdiff_t);
 374                 else if (lflag)
 375                     num = va_arg(ap, long);
 376                 else if (zflag)
 377                     num = va_arg(ap, ssize_t);
 378                 else if (hflag)
 379                     num = (short)va_arg(ap, int);
 380                 else if (cflag)
 381                     num = (char)va_arg(ap, int);
 382                 else
 383                     num = va_arg(ap, int);
 384 number:
 385                 if (sign && (intmax_t)num < 0) {
 386                     neg = 1;
 387                     num = -(intmax_t)num;
 388                 }
 389
 390                 p = ksprintn(nbuf, num, base, &tmp, upper);
 391
 392                 if (sharpflag && num != 0) {
 393                     if (base == 8) {
 394                         tmp++;
 395                     } else if (base == 16) {
 396                         tmp += 2;
 397                     }
 398                 }
 399
 400                 if (neg) {
 401                     tmp++;
 402                 }
 403
 404                 if (!ladjust && padc != '0' && width && (width -= tmp) > 0) {
 405                     while (width--) {
 406                         PCHAR(padc);
 407                     }
 408                 }
 409
 410                 if (neg) {
 411                     PCHAR('-');
 412                 }
 413
 414                 if (sharpflag && num != 0) {
 415                     if (base == 8) {
 416                         PCHAR('0');
 417                     } else if (base == 16) {
 418                         PCHAR('0');
 419                         PCHAR('x');
 420                     }
 421                 }
 422
 423                 if (!ladjust && width && (width -= tmp) > 0) {
 424                     while (width--) {
 425                         PCHAR(padc);
 426                     }
 427                 }
 428
 429                 while (*p) {
 430                     PCHAR(*p--);
 431                 }
 432
 433                 if (ladjust && width && (width -= tmp) > 0) {
 434                     while (width--) {
 435                         PCHAR(padc);
 436                     }
 437                 }
 438
 439                 break;
 440             default:
 441                 while (percent < fmt) {
 442                     PCHAR(*percent++);
 443                 }
 444                 /*
 445                  * Since we ignore an formatting argument it is no
 446                  * longer safe to obey the remaining formatting
 447                  * arguments as the arguments will no longer match
 448                  * the format specs.
 449                  */
 450                 stop = 1;
 451                 break;
 452         }
 453     }
 454     PCHAR(0);
 455     return retval;
 456 #undef PCHAR
 457 }
 458
 459 int vsprintf(char *dest, const char *fmt, va_list ap) {
 460     return kvsprintf(fmt, dest, 10, ap);
 461 }
 462
 463 int
 464 sprintf(char *dest, const char *fmt, ...) {
 465     /* http://www.pagetable.com/?p=298 */
 466     int retval;
 467     va_list ap;
 468     va_start(ap, fmt);
 469     retval = kvsprintf(fmt, dest, 10, ap);
 470     va_end(ap);
 471     return retval;
 472 }