tiny-printf.c 7.7 KB

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