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 } 236