lib/tiny-printf.c

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