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