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