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