sbi_console.c 7.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383
  1. /*
  2. * SPDX-License-Identifier: BSD-2-Clause
  3. *
  4. * Copyright (c) 2019 Western Digital Corporation or its affiliates.
  5. *
  6. * Authors:
  7. * Anup Patel <anup.patel@wdc.com>
  8. */
  9. #include <sbi/sbi_platform.h>
  10. #include <sbi/sbi_console.h>
  11. #include <sbi/riscv_locks.h>
  12. static const struct sbi_platform *console_plat = NULL;
  13. static spinlock_t console_out_lock = SPIN_LOCK_INITIALIZER;
  14. bool sbi_isprintable(char c)
  15. {
  16. if (((31 < c) && (c < 127)) || (c == '\f') || (c == '\r') ||
  17. (c == '\n') || (c == '\t')) {
  18. return TRUE;
  19. }
  20. return FALSE;
  21. }
  22. int sbi_getc(void)
  23. {
  24. return sbi_platform_console_getc(console_plat);
  25. }
  26. void sbi_putc(char ch)
  27. {
  28. if (ch == '\n')
  29. sbi_platform_console_putc(console_plat, '\r');
  30. sbi_platform_console_putc(console_plat, ch);
  31. }
  32. void sbi_puts(const char *str)
  33. {
  34. spin_lock(&console_out_lock);
  35. while (*str) {
  36. sbi_putc(*str);
  37. str++;
  38. }
  39. spin_unlock(&console_out_lock);
  40. }
  41. void sbi_gets(char *s, int maxwidth, char endchar)
  42. {
  43. int ch;
  44. char *retval = s;
  45. while ((ch = sbi_getc()) != endchar && ch >= 0 && maxwidth > 1) {
  46. *retval = (char)ch;
  47. retval++;
  48. maxwidth--;
  49. }
  50. *retval = '\0';
  51. }
  52. #define PAD_RIGHT 1
  53. #define PAD_ZERO 2
  54. #define PAD_ALTERNATE 4
  55. #define PRINT_BUF_LEN 64
  56. #define va_start(v, l) __builtin_va_start((v), l)
  57. #define va_end __builtin_va_end
  58. #define va_arg __builtin_va_arg
  59. typedef __builtin_va_list va_list;
  60. static void printc(char **out, u32 *out_len, char ch)
  61. {
  62. if (out) {
  63. if (*out) {
  64. if (out_len && (0 < *out_len)) {
  65. **out = ch;
  66. ++(*out);
  67. (*out_len)--;
  68. } else {
  69. **out = ch;
  70. ++(*out);
  71. }
  72. }
  73. } else {
  74. sbi_putc(ch);
  75. }
  76. }
  77. static int prints(char **out, u32 *out_len, const char *string, int width,
  78. int flags)
  79. {
  80. int pc = 0;
  81. char padchar = ' ';
  82. if (width > 0) {
  83. int len = 0;
  84. const char *ptr;
  85. for (ptr = string; *ptr; ++ptr)
  86. ++len;
  87. if (len >= width)
  88. width = 0;
  89. else
  90. width -= len;
  91. if (flags & PAD_ZERO)
  92. padchar = '0';
  93. }
  94. if (!(flags & PAD_RIGHT)) {
  95. for (; width > 0; --width) {
  96. printc(out, out_len, padchar);
  97. ++pc;
  98. }
  99. }
  100. for (; *string; ++string) {
  101. printc(out, out_len, *string);
  102. ++pc;
  103. }
  104. for (; width > 0; --width) {
  105. printc(out, out_len, padchar);
  106. ++pc;
  107. }
  108. return pc;
  109. }
  110. static int printi(char **out, u32 *out_len, long long i, int b, int sg,
  111. int width, int flags, int letbase)
  112. {
  113. char print_buf[PRINT_BUF_LEN];
  114. char *s;
  115. int neg = 0, pc = 0;
  116. u64 t;
  117. unsigned long long u = i;
  118. if (sg && b == 10 && i < 0) {
  119. neg = 1;
  120. u = -i;
  121. }
  122. s = print_buf + PRINT_BUF_LEN - 1;
  123. *s = '\0';
  124. if (!u) {
  125. *--s = '0';
  126. } else {
  127. while (u) {
  128. t = u % b;
  129. u = u / b;
  130. if (t >= 10)
  131. t += letbase - '0' - 10;
  132. *--s = t + '0';
  133. }
  134. }
  135. if (flags & PAD_ALTERNATE) {
  136. if ((b == 16) && (letbase == 'A')) {
  137. *--s = 'X';
  138. } else if ((b == 16) && (letbase == 'a')) {
  139. *--s = 'x';
  140. }
  141. *--s = '0';
  142. }
  143. if (neg) {
  144. if (width && (flags & PAD_ZERO)) {
  145. printc(out, out_len, '-');
  146. ++pc;
  147. --width;
  148. } else {
  149. *--s = '-';
  150. }
  151. }
  152. return pc + prints(out, out_len, s, width, flags);
  153. }
  154. static int print(char **out, u32 *out_len, const char *format, va_list args)
  155. {
  156. int width, flags, acnt = 0;
  157. int pc = 0;
  158. char scr[2];
  159. unsigned long long tmp;
  160. for (; *format != 0; ++format) {
  161. if (*format == '%') {
  162. ++format;
  163. width = flags = 0;
  164. if (*format == '\0')
  165. break;
  166. if (*format == '%')
  167. goto out;
  168. /* Get flags */
  169. if (*format == '-') {
  170. ++format;
  171. flags = PAD_RIGHT;
  172. }
  173. if (*format == '#') {
  174. ++format;
  175. flags |= PAD_ALTERNATE;
  176. }
  177. while (*format == '0') {
  178. ++format;
  179. flags |= PAD_ZERO;
  180. }
  181. /* Get width */
  182. for (; *format >= '0' && *format <= '9'; ++format) {
  183. width *= 10;
  184. width += *format - '0';
  185. }
  186. if (*format == 's') {
  187. char *s = va_arg(args, char *);
  188. acnt += sizeof(char *);
  189. pc += prints(out, out_len, s ? s : "(null)",
  190. width, flags);
  191. continue;
  192. }
  193. if ((*format == 'd') || (*format == 'i')) {
  194. pc += printi(out, out_len, va_arg(args, int),
  195. 10, 1, width, flags, '0');
  196. acnt += sizeof(int);
  197. continue;
  198. }
  199. if (*format == 'x') {
  200. pc += printi(out, out_len,
  201. va_arg(args, unsigned int), 16, 0,
  202. width, flags, 'a');
  203. acnt += sizeof(unsigned int);
  204. continue;
  205. }
  206. if (*format == 'X') {
  207. pc += printi(out, out_len,
  208. va_arg(args, unsigned int), 16, 0,
  209. width, flags, 'A');
  210. acnt += sizeof(unsigned int);
  211. continue;
  212. }
  213. if (*format == 'u') {
  214. pc += printi(out, out_len,
  215. va_arg(args, unsigned int), 10, 0,
  216. width, flags, 'a');
  217. acnt += sizeof(unsigned int);
  218. continue;
  219. }
  220. if (*format == 'p') {
  221. pc += printi(out, out_len,
  222. va_arg(args, unsigned long), 16, 0,
  223. width, flags, 'a');
  224. acnt += sizeof(unsigned long);
  225. continue;
  226. }
  227. if (*format == 'P') {
  228. pc += printi(out, out_len,
  229. va_arg(args, unsigned long), 16, 0,
  230. width, flags, 'A');
  231. acnt += sizeof(unsigned long);
  232. continue;
  233. }
  234. if (*format == 'l' && *(format + 1) == 'l') {
  235. while (acnt &
  236. (sizeof(unsigned long long) - 1)) {
  237. va_arg(args, int);
  238. acnt += sizeof(int);
  239. }
  240. if (sizeof(unsigned long long) ==
  241. sizeof(unsigned long)) {
  242. tmp = va_arg(args, unsigned long long);
  243. acnt += sizeof(unsigned long long);
  244. } else {
  245. ((unsigned long *)&tmp)[0] =
  246. va_arg(args, unsigned long);
  247. ((unsigned long *)&tmp)[1] =
  248. va_arg(args, unsigned long);
  249. acnt += 2 * sizeof(unsigned long);
  250. }
  251. if (*(format + 2) == 'u') {
  252. format += 2;
  253. pc += printi(out, out_len, tmp, 10, 0,
  254. width, flags, 'a');
  255. } else if (*(format + 2) == 'x') {
  256. format += 2;
  257. pc += printi(out, out_len, tmp, 16, 0,
  258. width, flags, 'a');
  259. } else if (*(format + 2) == 'X') {
  260. format += 2;
  261. pc += printi(out, out_len, tmp, 16, 0,
  262. width, flags, 'A');
  263. } else {
  264. format += 1;
  265. pc += printi(out, out_len, tmp, 10, 1,
  266. width, flags, '0');
  267. }
  268. continue;
  269. } else if (*format == 'l') {
  270. if (*(format + 1) == 'u') {
  271. format += 1;
  272. pc += printi(
  273. out, out_len,
  274. va_arg(args, unsigned long), 10,
  275. 0, width, flags, 'a');
  276. } else if (*(format + 1) == 'x') {
  277. format += 1;
  278. pc += printi(
  279. out, out_len,
  280. va_arg(args, unsigned long), 16,
  281. 0, width, flags, 'a');
  282. acnt += sizeof(unsigned long);
  283. } else if (*(format + 1) == 'X') {
  284. format += 1;
  285. pc += printi(
  286. out, out_len,
  287. va_arg(args, unsigned long), 16,
  288. 0, width, flags, 'A');
  289. acnt += sizeof(unsigned long);
  290. } else {
  291. pc += printi(out, out_len,
  292. va_arg(args, long), 10, 1,
  293. width, flags, '0');
  294. acnt += sizeof(long);
  295. }
  296. }
  297. if (*format == 'c') {
  298. /* char are converted to int then pushed on the stack */
  299. scr[0] = va_arg(args, int);
  300. scr[1] = '\0';
  301. pc += prints(out, out_len, scr, width, flags);
  302. acnt += sizeof(int);
  303. continue;
  304. }
  305. } else {
  306. out:
  307. printc(out, out_len, *format);
  308. ++pc;
  309. }
  310. }
  311. if (out)
  312. **out = '\0';
  313. return pc;
  314. }
  315. int sbi_sprintf(char *out, const char *format, ...)
  316. {
  317. va_list args;
  318. int retval;
  319. va_start(args, format);
  320. retval = print(&out, NULL, format, args);
  321. va_end(args);
  322. return retval;
  323. }
  324. int sbi_snprintf(char *out, u32 out_sz, const char *format, ...)
  325. {
  326. va_list args;
  327. int retval;
  328. va_start(args, format);
  329. retval = print(&out, &out_sz, format, args);
  330. va_end(args);
  331. return retval;
  332. }
  333. int sbi_printf(const char *format, ...)
  334. {
  335. va_list args;
  336. int retval;
  337. spin_lock(&console_out_lock);
  338. va_start(args, format);
  339. retval = print(NULL, NULL, format, args);
  340. va_end(args);
  341. spin_unlock(&console_out_lock);
  342. return retval;
  343. }
  344. int sbi_console_init(struct sbi_scratch *scratch)
  345. {
  346. console_plat = sbi_platform_ptr(scratch);
  347. return sbi_platform_console_init(console_plat);
  348. }