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