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
  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 void putc_normal(struct printf_info *info, char ch)
  26 {
  27         putc(ch);
  28 }
  29
  30 static void out(struct printf_info *info, char c)
  31 {
  32         *info->bf++ = c;
  33 }
  34
  35 static void out_dgt(struct printf_info *info, char dgt)
  36 {
  37         out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
  38         info->zs = 1;
  39 }
  40
  41 static void div_out(struct printf_info *info, unsigned long *num,
  42                     unsigned long div)
  43 {
  44         unsigned char dgt = 0;
  45
  46         while (*num >= div) {
  47                 *num -= div;
  48                 dgt++;
  49         }
  50
  51         if (info->zs || dgt > 0)
  52                 out_dgt(info, dgt);
  53 }
  54
  55 int _vprintf(struct printf_info *info, const char *fmt, va_list va)
  56 {
  57         char ch;
  58         char *p;
  59         unsigned long num;
  60         char buf[12];
  61         unsigned long div;
  62
  63         while ((ch = *(fmt++))) {
  64                 if (ch != '%') {
  65                         info->putc(info, ch);
  66                 } else {
  67                         bool lz = false;
  68                         int width = 0;
  69                         bool islong = false;
  70
  71                         ch = *(fmt++);
  72                         if (ch == '0') {
  73                                 ch = *(fmt++);
  74                                 lz = 1;
  75                         }
  76
  77                         if (ch >= '0' && ch <= '9') {
  78                                 width = 0;
  79                                 while (ch >= '0' && ch <= '9') {
  80                                         width = (width * 10) + ch - '0';
  81                                         ch = *fmt++;
  82                                 }
  83                         }
  84                         if (ch == 'l') {
  85                                 ch = *(fmt++);
  86                                 islong = true;
  87                         }
  88
  89                         info->bf = buf;
  90                         p = info->bf;
  91                         info->zs = 0;
  92
  93                         switch (ch) {
  94                         case '\0':
  95                                 goto abort;
  96                         case 'u':
  97                         case 'd':
  98                                 div = 1000000000;
  99                                 if (islong) {
 100                                         num = va_arg(va, unsigned long);
 101                                         if (sizeof(long) > 4)
 102                                                 div *= div * 10;
 103                                 } else {
 104                                         num = va_arg(va, unsigned int);
 105                                 }
 106
 107                                 if (ch == 'd') {
 108                                         if (islong && (long)num < 0) {
 109                                                 num = -(long)num;
 110                                                 out(info, '-');
 111                                         } else if (!islong && (int)num < 0) {
 112                                                 num = -(int)num;
 113                                                 out(info, '-');
 114                                         }
 115                                 }
 116                                 if (!num) {
 117                                         out_dgt(info, 0);
 118                                 } else {
 119                                         for (; div; div /= 10)
 120                                                 div_out(info, &num, div);
 121                                 }
 122                                 break;
 123                         case 'x':
 124                                 if (islong) {
 125                                         num = va_arg(va, unsigned long);
 126                                         div = 1UL << (sizeof(long) * 8 - 4);
 127                                 } else {
 128                                         num = va_arg(va, unsigned int);
 129                                         div = 0x10000000;
 130                                 }
 131                                 if (!num) {
 132                                         out_dgt(info, 0);
 133                                 } else {
 134                                         for (; div; div /= 0x10)
 135                                                 div_out(info, &num, div);
 136                                 }
 137                                 break;
 138                         case 'c':
 139                                 out(info, (char)(va_arg(va, int)));
 140                                 break;
 141                         case 's':
 142                                 p = va_arg(va, char*);
 143                                 break;
 144                         case '%':
 145                                 out(info, '%');
 146                         default:
 147                                 break;
 148                         }
 149
 150                         *info->bf = 0;
 151                         info->bf = p;
 152                         while (*info->bf++ && width > 0)
 153                                 width--;
 154                         while (width-- > 0)
 155                                 info->putc(info, lz ? '0' : ' ');
 156                         if (p) {
 157                                 while ((ch = *p++))
 158                                         info->putc(info, ch);
 159                         }
 160                 }
 161         }
 162
 163 abort:
 164         return 0;
 165 }
 166
 167 int vprintf(const char *fmt, va_list va)
 168 {
 169         struct printf_info info;
 170
 171         info.putc = putc_normal;
 172         return _vprintf(&info, fmt, va);
 173 }
 174
 175 int printf(const char *fmt, ...)
 176 {
 177         struct printf_info info;
 178
 179         va_list va;
 180         int ret;
 181
 182         info.putc = putc_normal;
 183         va_start(va, fmt);
 184         ret = _vprintf(&info, fmt, va);
 185         va_end(va);
 186
 187         return ret;
 188 }
 189
 190 static void putc_outstr(struct printf_info *info, char ch)
 191 {
 192         *info->outstr++ = ch;
 193 }
 194
 195 int sprintf(char *buf, const char *fmt, ...)
 196 {
 197         struct printf_info info;
 198         va_list va;
 199         int ret;
 200
 201         va_start(va, fmt);
 202         info.outstr = buf;
 203         info.putc = putc_outstr;
 204         ret = _vprintf(&info, fmt, va);
 205         va_end(va);
 206         *info.outstr = '\0';
 207
 208         return ret;
 209 }
 210
 211 /* Note that size is ignored */
 212 int snprintf(char *buf, size_t size, const char *fmt, ...)
 213 {
 214         struct printf_info info;
 215         va_list va;
 216         int ret;
 217
 218         va_start(va, fmt);
 219         info.outstr = buf;
 220         info.putc = putc_outstr;
 221         ret = _vprintf(&info, fmt, va);
 222         va_end(va);
 223         *info.outstr = '\0';
 224
 225         return ret;
 226 }
 227
 228 void __assert_fail(const char *assertion, const char *file, unsigned line,
 229                    const char *function)
 230 {
 231         /* This will not return */
 232         printf("%s:%u: %s: Assertion `%s' failed.", file, line, function,
 233                assertion);
 234         hang();
 235 }