tiny-printf.c 7.8 KB

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