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 #include <linux/ctype.h> 16 17 struct printf_info { 18 char *bf; /* Digit buffer */ 19 char zs; /* non-zero if a digit has been written */ 20 char *outstr; /* Next output position for sprintf() */ 21 22 /* Output a character */ 23 void (*putc)(struct printf_info *info, char ch); 24 }; 25 26 static void putc_normal(struct printf_info *info, char ch) 27 { 28 putc(ch); 29 } 30 31 static void out(struct printf_info *info, char c) 32 { 33 *info->bf++ = c; 34 } 35 36 static void out_dgt(struct printf_info *info, char dgt) 37 { 38 out(info, dgt + (dgt < 10 ? '0' : 'a' - 10)); 39 info->zs = 1; 40 } 41 42 static void div_out(struct printf_info *info, unsigned long *num, 43 unsigned long div) 44 { 45 unsigned char dgt = 0; 46 47 while (*num >= div) { 48 *num -= div; 49 dgt++; 50 } 51 52 if (info->zs || dgt > 0) 53 out_dgt(info, dgt); 54 } 55 56 #ifdef CONFIG_SPL_NET_SUPPORT 57 static void string(struct printf_info *info, char *s) 58 { 59 char ch; 60 61 while ((ch = *s++)) 62 out(info, ch); 63 } 64 65 static const char hex_asc[] = "0123456789abcdef"; 66 #define hex_asc_lo(x) hex_asc[((x) & 0x0f)] 67 #define hex_asc_hi(x) hex_asc[((x) & 0xf0) >> 4] 68 69 static inline char *pack_hex_byte(char *buf, u8 byte) 70 { 71 *buf++ = hex_asc_hi(byte); 72 *buf++ = hex_asc_lo(byte); 73 return buf; 74 } 75 76 static void mac_address_string(struct printf_info *info, u8 *addr, 77 bool separator) 78 { 79 /* (6 * 2 hex digits), 5 colons and trailing zero */ 80 char mac_addr[6 * 3]; 81 char *p = mac_addr; 82 int i; 83 84 for (i = 0; i < 6; i++) { 85 p = pack_hex_byte(p, addr[i]); 86 if (separator && i != 5) 87 *p++ = ':'; 88 } 89 *p = '\0'; 90 91 string(info, mac_addr); 92 } 93 94 static char *put_dec_trunc(char *buf, unsigned int q) 95 { 96 unsigned int d3, d2, d1, d0; 97 d1 = (q >> 4) & 0xf; 98 d2 = (q >> 8) & 0xf; 99 d3 = (q >> 12); 100 101 d0 = 6 * (d3 + d2 + d1) + (q & 0xf); 102 q = (d0 * 0xcd) >> 11; 103 d0 = d0 - 10 * q; 104 *buf++ = d0 + '0'; /* least significant digit */ 105 d1 = q + 9 * d3 + 5 * d2 + d1; 106 if (d1 != 0) { 107 q = (d1 * 0xcd) >> 11; 108 d1 = d1 - 10 * q; 109 *buf++ = d1 + '0'; /* next digit */ 110 111 d2 = q + 2 * d2; 112 if ((d2 != 0) || (d3 != 0)) { 113 q = (d2 * 0xd) >> 7; 114 d2 = d2 - 10 * q; 115 *buf++ = d2 + '0'; /* next digit */ 116 117 d3 = q + 4 * d3; 118 if (d3 != 0) { 119 q = (d3 * 0xcd) >> 11; 120 d3 = d3 - 10 * q; 121 *buf++ = d3 + '0'; /* next digit */ 122 if (q != 0) 123 *buf++ = q + '0'; /* most sign. digit */ 124 } 125 } 126 } 127 return buf; 128 } 129 130 static void ip4_addr_string(struct printf_info *info, u8 *addr) 131 { 132 /* (4 * 3 decimal digits), 3 dots and trailing zero */ 133 char ip4_addr[4 * 4]; 134 char temp[3]; /* hold each IP quad in reverse order */ 135 char *p = ip4_addr; 136 int i, digits; 137 138 for (i = 0; i < 4; i++) { 139 digits = put_dec_trunc(temp, addr[i]) - temp; 140 /* reverse the digits in the quad */ 141 while (digits--) 142 *p++ = temp[digits]; 143 if (i != 3) 144 *p++ = '.'; 145 } 146 *p = '\0'; 147 148 string(info, ip4_addr); 149 } 150 #endif 151 152 /* 153 * Show a '%p' thing. A kernel extension is that the '%p' is followed 154 * by an extra set of characters that are extended format 155 * specifiers. 156 * 157 * Right now we handle: 158 * 159 * - 'M' For a 6-byte MAC address, it prints the address in the 160 * usual colon-separated hex notation. 161 * - 'm' Same as above except there is no colon-separator. 162 * - 'I4'for IPv4 addresses printed in the usual way (dot-separated 163 * decimal). 164 */ 165 166 static void pointer(struct printf_info *info, const char *fmt, void *ptr) 167 { 168 #ifdef DEBUG 169 unsigned long num = (uintptr_t)ptr; 170 unsigned long div; 171 #endif 172 173 switch (*fmt) { 174 #ifdef DEBUG 175 case 'a': 176 177 switch (fmt[1]) { 178 case 'p': 179 default: 180 num = *(phys_addr_t *)ptr; 181 break; 182 } 183 break; 184 #endif 185 #ifdef CONFIG_SPL_NET_SUPPORT 186 case 'm': 187 return mac_address_string(info, ptr, false); 188 case 'M': 189 return mac_address_string(info, ptr, true); 190 case 'I': 191 if (fmt[1] == '4') 192 return ip4_addr_string(info, ptr); 193 #endif 194 default: 195 break; 196 } 197 #ifdef DEBUG 198 div = 1UL << (sizeof(long) * 8 - 4); 199 for (; div; div /= 0x10) 200 div_out(info, &num, div); 201 #endif 202 } 203 204 static int _vprintf(struct printf_info *info, const char *fmt, va_list va) 205 { 206 char ch; 207 char *p; 208 unsigned long num; 209 char buf[12]; 210 unsigned long div; 211 212 while ((ch = *(fmt++))) { 213 if (ch != '%') { 214 info->putc(info, ch); 215 } else { 216 bool lz = false; 217 int width = 0; 218 bool islong = false; 219 220 ch = *(fmt++); 221 if (ch == '-') 222 ch = *(fmt++); 223 224 if (ch == '0') { 225 ch = *(fmt++); 226 lz = 1; 227 } 228 229 if (ch >= '0' && ch <= '9') { 230 width = 0; 231 while (ch >= '0' && ch <= '9') { 232 width = (width * 10) + ch - '0'; 233 ch = *fmt++; 234 } 235 } 236 if (ch == 'l') { 237 ch = *(fmt++); 238 islong = true; 239 } 240 241 info->bf = buf; 242 p = info->bf; 243 info->zs = 0; 244 245 switch (ch) { 246 case '\0': 247 goto abort; 248 case 'u': 249 case 'd': 250 div = 1000000000; 251 if (islong) { 252 num = va_arg(va, unsigned long); 253 if (sizeof(long) > 4) 254 div *= div * 10; 255 } else { 256 num = va_arg(va, unsigned int); 257 } 258 259 if (ch == 'd') { 260 if (islong && (long)num < 0) { 261 num = -(long)num; 262 out(info, '-'); 263 } else if (!islong && (int)num < 0) { 264 num = -(int)num; 265 out(info, '-'); 266 } 267 } 268 if (!num) { 269 out_dgt(info, 0); 270 } else { 271 for (; div; div /= 10) 272 div_out(info, &num, div); 273 } 274 break; 275 case 'x': 276 if (islong) { 277 num = va_arg(va, unsigned long); 278 div = 1UL << (sizeof(long) * 8 - 4); 279 } else { 280 num = va_arg(va, unsigned int); 281 div = 0x10000000; 282 } 283 if (!num) { 284 out_dgt(info, 0); 285 } else { 286 for (; div; div /= 0x10) 287 div_out(info, &num, div); 288 } 289 break; 290 case 'c': 291 out(info, (char)(va_arg(va, int))); 292 break; 293 case 's': 294 p = va_arg(va, char*); 295 break; 296 case 'p': 297 pointer(info, fmt, va_arg(va, void *)); 298 while (isalnum(fmt[0])) 299 fmt++; 300 break; 301 case '%': 302 out(info, '%'); 303 default: 304 break; 305 } 306 307 *info->bf = 0; 308 info->bf = p; 309 while (*info->bf++ && width > 0) 310 width--; 311 while (width-- > 0) 312 info->putc(info, lz ? '0' : ' '); 313 if (p) { 314 while ((ch = *p++)) 315 info->putc(info, ch); 316 } 317 } 318 } 319 320 abort: 321 return 0; 322 } 323 324 int vprintf(const char *fmt, va_list va) 325 { 326 struct printf_info info; 327 328 info.putc = putc_normal; 329 return _vprintf(&info, fmt, va); 330 } 331 332 int printf(const char *fmt, ...) 333 { 334 struct printf_info info; 335 336 va_list va; 337 int ret; 338 339 info.putc = putc_normal; 340 va_start(va, fmt); 341 ret = _vprintf(&info, fmt, va); 342 va_end(va); 343 344 return ret; 345 } 346 347 static void putc_outstr(struct printf_info *info, char ch) 348 { 349 *info->outstr++ = ch; 350 } 351 352 int sprintf(char *buf, const char *fmt, ...) 353 { 354 struct printf_info info; 355 va_list va; 356 int ret; 357 358 va_start(va, fmt); 359 info.outstr = buf; 360 info.putc = putc_outstr; 361 ret = _vprintf(&info, fmt, va); 362 va_end(va); 363 *info.outstr = '\0'; 364 365 return ret; 366 } 367 368 /* Note that size is ignored */ 369 int snprintf(char *buf, size_t size, const char *fmt, ...) 370 { 371 struct printf_info info; 372 va_list va; 373 int ret; 374 375 va_start(va, fmt); 376 info.outstr = buf; 377 info.putc = putc_outstr; 378 ret = _vprintf(&info, fmt, va); 379 va_end(va); 380 *info.outstr = '\0'; 381 382 return ret; 383 } 384 385 void __assert_fail(const char *assertion, const char *file, unsigned line, 386 const char *function) 387 { 388 /* This will not return */ 389 printf("%s:%u: %s: Assertion `%s' failed.", file, line, function, 390 assertion); 391 hang(); 392 } 393