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