lib/tiny-printf.c

   1 // SPDX-License-Identifier: LGPL-2.1+
   2 /*
   3  * Tiny printf version for SPL
   4  *
   5  * Copied from:
   6  * http://www.sparetimelabs.com/printfrevisited/printfrevisited.php
   7  *
   8  * Copyright (C) 2004,2008  Kustaa Nyholm
   9  */
  10
  11 #include <common.h>
  12 #include <log.h>
  13 #include <serial.h>
  14 #include <stdarg.h>
  15 #include <linux/ctype.h>
  16
  17 struct printf_info {
  18         char *bf;       /* Digit buffer */
  19         char zs;        /* non-zero if a digit has been written */
  20         char *outstr;   /* Next output position for sprintf() */
  21
  22         /* Output a character */
  23         void (*putc)(struct printf_info *info, char ch);
  24 };
  25
  26 static void out(struct printf_info *info, char c)
  27 {
  28         *info->bf++ = c;
  29 }
  30
  31 static void out_dgt(struct printf_info *info, char dgt)
  32 {
  33         out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
  34         info->zs = 1;
  35 }
  36
  37 static void div_out(struct printf_info *info, unsigned long *num,
  38                     unsigned long div)
  39 {
  40         unsigned char dgt = 0;
  41
  42         while (*num >= div) {
  43                 *num -= div;
  44                 dgt++;
  45         }
  46
  47         if (info->zs || dgt > 0)
  48                 out_dgt(info, dgt);
  49 }
  50
  51 #ifdef CONFIG_SPL_NET
  52 static void string(struct printf_info *info, char *s)
  53 {
  54         char ch;
  55
  56         while ((ch = *s++))
  57                 out(info, ch);
  58 }
  59
  60 static const char hex_asc[] = "0123456789abcdef";
  61 #define hex_asc_lo(x)   hex_asc[((x) & 0x0f)]
  62 #define hex_asc_hi(x)   hex_asc[((x) & 0xf0) >> 4]
  63
  64 static inline char *pack_hex_byte(char *buf, u8 byte)
  65 {
  66         *buf++ = hex_asc_hi(byte);
  67         *buf++ = hex_asc_lo(byte);
  68         return buf;
  69 }
  70
  71 static void mac_address_string(struct printf_info *info, u8 *addr,
  72                                 bool separator)
  73 {
  74         /* (6 * 2 hex digits), 5 colons and trailing zero */
  75         char mac_addr[6 * 3];
  76         char *p = mac_addr;
  77         int i;
  78
  79         for (i = 0; i < 6; i++) {
  80                 p = pack_hex_byte(p, addr[i]);
  81                 if (separator && i != 5)
  82                         *p++ = ':';
  83         }
  84         *p = '\0';
  85
  86         string(info, mac_addr);
  87 }
  88
  89 static char *put_dec_trunc(char *buf, unsigned int q)
  90 {
  91         unsigned int d3, d2, d1, d0;
  92         d1 = (q >> 4) & 0xf;
  93         d2 = (q >> 8) & 0xf;
  94         d3 = (q >> 12);
  95
  96         d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
  97         q = (d0 * 0xcd) >> 11;
  98         d0 = d0 - 10 * q;
  99         *buf++ = d0 + '0'; /* least significant digit */
 100         d1 = q + 9 * d3 + 5 * d2 + d1;
 101         if (d1 != 0) {
 102                 q = (d1 * 0xcd) >> 11;
 103                 d1 = d1 - 10 * q;
 104                 *buf++ = d1 + '0'; /* next digit */
 105
 106                 d2 = q + 2 * d2;
 107                 if ((d2 != 0) || (d3 != 0)) {
 108                         q = (d2 * 0xd) >> 7;
 109                         d2 = d2 - 10 * q;
 110                         *buf++ = d2 + '0'; /* next digit */
 111
 112                         d3 = q + 4 * d3;
 113                         if (d3 != 0) {
 114                                 q = (d3 * 0xcd) >> 11;
 115                                 d3 = d3 - 10 * q;
 116                                 *buf++ = d3 + '0';  /* next digit */
 117                                 if (q != 0)
 118                                         *buf++ = q + '0'; /* most sign. digit */
 119                         }
 120                 }
 121         }
 122         return buf;
 123 }
 124
 125 static void ip4_addr_string(struct printf_info *info, u8 *addr)
 126 {
 127         /* (4 * 3 decimal digits), 3 dots and trailing zero */
 128         char ip4_addr[4 * 4];
 129         char temp[3];   /* hold each IP quad in reverse order */
 130         char *p = ip4_addr;
 131         int i, digits;
 132
 133         for (i = 0; i < 4; i++) {
 134                 digits = put_dec_trunc(temp, addr[i]) - temp;
 135                 /* reverse the digits in the quad */
 136                 while (digits--)
 137                         *p++ = temp[digits];
 138                 if (i != 3)
 139                         *p++ = '.';
 140         }
 141         *p = '\0';
 142
 143         string(info, ip4_addr);
 144 }
 145 #endif
 146
 147 /*
 148  * Show a '%p' thing.  A kernel extension is that the '%p' is followed
 149  * by an extra set of characters that are extended format
 150  * specifiers.
 151  *
 152  * Right now we handle:
 153  *
 154  * - 'M' For a 6-byte MAC address, it prints the address in the
 155  *       usual colon-separated hex notation.
 156  * - 'm' Same as above except there is no colon-separator.
 157  * - 'I4'for IPv4 addresses printed in the usual way (dot-separated
 158  *       decimal).
 159  */
 160
 161 static void __maybe_unused pointer(struct printf_info *info, const char *fmt,
 162                                    void *ptr)
 163 {
 164 #ifdef DEBUG
 165         unsigned long num = (uintptr_t)ptr;
 166         unsigned long div;
 167 #endif
 168
 169         switch (*fmt) {
 170 #ifdef DEBUG
 171         case 'a':
 172
 173                 switch (fmt[1]) {
 174                 case 'p':
 175                 default:
 176                         num = *(phys_addr_t *)ptr;
 177                         break;
 178                 }
 179                 break;
 180 #endif
 181 #ifdef CONFIG_SPL_NET
 182         case 'm':
 183                 return mac_address_string(info, ptr, false);
 184         case 'M':
 185                 return mac_address_string(info, ptr, true);
 186         case 'I':
 187                 if (fmt[1] == '4')
 188                         return ip4_addr_string(info, ptr);
 189 #endif
 190         default:
 191                 break;
 192         }
 193 #ifdef DEBUG
 194         div = 1UL << (sizeof(long) * 8 - 4);
 195         for (; div; div /= 0x10)
 196                 div_out(info, &num, div);
 197 #endif
 198 }
 199
 200 static int _vprintf(struct printf_info *info, const char *fmt, va_list va)
 201 {
 202         char ch;
 203         char *p;
 204         unsigned long num;
 205         char buf[12];
 206         unsigned long div;
 207
 208         while ((ch = *(fmt++))) {
 209                 if (ch != '%') {
 210                         info->putc(info, ch);
 211                 } else {
 212                         bool lz = false;
 213                         int width = 0;
 214                         bool islong = false;
 215
 216                         ch = *(fmt++);
 217                         if (ch == '-')
 218                                 ch = *(fmt++);
 219
 220                         if (ch == '0') {
 221                                 ch = *(fmt++);
 222                                 lz = 1;
 223                         }
 224
 225                         if (ch >= '0' && ch <= '9') {
 226                                 width = 0;
 227                                 while (ch >= '0' && ch <= '9') {
 228                                         width = (width * 10) + ch - '0';
 229                                         ch = *fmt++;
 230                                 }
 231                         }
 232                         if (ch == 'l') {
 233                                 ch = *(fmt++);
 234                                 islong = true;
 235                         }
 236
 237                         info->bf = buf;
 238                         p = info->bf;
 239                         info->zs = 0;
 240
 241                         switch (ch) {
 242                         case '\0':
 243                                 goto abort;
 244                         case 'u':
 245                         case 'd':
 246                         case 'i':
 247                                 div = 1000000000;
 248                                 if (islong) {
 249                                         num = va_arg(va, unsigned long);
 250                                         if (sizeof(long) > 4)
 251                                                 div *= div * 10;
 252                                 } else {
 253                                         num = va_arg(va, unsigned int);
 254                                 }
 255
 256                                 if (ch != 'u') {
 257                                         if (islong && (long)num < 0) {
 258                                                 num = -(long)num;
 259                                                 out(info, '-');
 260                                         } else if (!islong && (int)num < 0) {
 261                                                 num = -(int)num;
 262                                                 out(info, '-');
 263                                         }
 264                                 }
 265                                 if (!num) {
 266                                         out_dgt(info, 0);
 267                                 } else {
 268                                         for (; div; div /= 10)
 269                                                 div_out(info, &num, div);
 270                                 }
 271                                 break;
 272                         case 'p':
 273                                 if (CONFIG_IS_ENABLED(NET) || _DEBUG) {
 274                                         pointer(info, fmt, va_arg(va, void *));
 275                                         /*
 276                                          * Skip this because it pulls in _ctype which is
 277                                          * 256 bytes, and we don't generally implement
 278                                          * pointer anyway
 279                                          */
 280                                         while (isalnum(fmt[0]))
 281                                                 fmt++;
 282                                         break;
 283                                 }
 284                                 islong = true;
 285                                 /* no break */
 286                         case 'x':
 287                                 if (islong) {
 288                                         num = va_arg(va, unsigned long);
 289                                         div = 1UL << (sizeof(long) * 8 - 4);
 290                                 } else {
 291                                         num = va_arg(va, unsigned int);
 292                                         div = 0x10000000;
 293                                 }
 294                                 if (!num) {
 295                                         out_dgt(info, 0);
 296                                 } else {
 297                                         for (; div; div /= 0x10)
 298                                                 div_out(info, &num, div);
 299                                 }
 300                                 break;
 301                         case 'c':
 302                                 out(info, (char)(va_arg(va, int)));
 303                                 break;
 304                         case 's':
 305                                 p = va_arg(va, char*);
 306                                 break;
 307                         case '%':
 308                                 out(info, '%');
 309                         default:
 310                                 break;
 311                         }
 312
 313                         *info->bf = 0;
 314                         info->bf = p;
 315                         while (*info->bf++ && width > 0)
 316                                 width--;
 317                         while (width-- > 0)
 318                                 info->putc(info, lz ? '0' : ' ');
 319                         if (p) {
 320                                 while ((ch = *p++))
 321                                         info->putc(info, ch);
 322                         }
 323                 }
 324         }
 325
 326 abort:
 327         return 0;
 328 }
 329
 330 #if CONFIG_IS_ENABLED(PRINTF)
 331 static void putc_normal(struct printf_info *info, char ch)
 332 {
 333         putc(ch);
 334 }
 335
 336 int vprintf(const char *fmt, va_list va)
 337 {
 338         struct printf_info info;
 339
 340         info.putc = putc_normal;
 341         return _vprintf(&info, fmt, va);
 342 }
 343
 344 int printf(const char *fmt, ...)
 345 {
 346         struct printf_info info;
 347
 348         va_list va;
 349         int ret;
 350
 351         info.putc = putc_normal;
 352         va_start(va, fmt);
 353         ret = _vprintf(&info, fmt, va);
 354         va_end(va);
 355
 356         return ret;
 357 }
 358 #endif
 359
 360 static void putc_outstr(struct printf_info *info, char ch)
 361 {
 362         *info->outstr++ = ch;
 363 }
 364
 365 int sprintf(char *buf, const char *fmt, ...)
 366 {
 367         struct printf_info info;
 368         va_list va;
 369         int ret;
 370
 371         va_start(va, fmt);
 372         info.outstr = buf;
 373         info.putc = putc_outstr;
 374         ret = _vprintf(&info, fmt, va);
 375         va_end(va);
 376         *info.outstr = '\0';
 377
 378         return ret;
 379 }
 380
 381 #if CONFIG_IS_ENABLED(LOG)
 382 /* Note that size is ignored */
 383 int vsnprintf(char *buf, size_t size, const char *fmt, va_list va)
 384 {
 385         struct printf_info info;
 386         int ret;
 387
 388         info.outstr = buf;
 389         info.putc = putc_outstr;
 390         ret = _vprintf(&info, fmt, va);
 391         *info.outstr = '\0';
 392
 393         return ret;
 394 }
 395 #endif
 396
 397 /* Note that size is ignored */
 398 int snprintf(char *buf, size_t size, const char *fmt, ...)
 399 {
 400         struct printf_info info;
 401         va_list va;
 402         int ret;
 403
 404         va_start(va, fmt);
 405         info.outstr = buf;
 406         info.putc = putc_outstr;
 407         ret = _vprintf(&info, fmt, va);
 408         va_end(va);
 409         *info.outstr = '\0';
 410
 411         return ret;
 412 }
 413
 414 void print_grouped_ull(unsigned long long int_val, int digits)
 415 {
 416         /* Don't try to print the upper 32-bits */
 417         printf("%ld ", (ulong)int_val);
 418 }